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ABSTRACT 


This  work  investigates  the  problem  of  robotic  arm  control  with  the  goal  of 
achieving  given  performance  requirements  by  solving  for  the  optimal  joint 
trajectories  and  corresponding  controls  for  tasks,  such  as  point-to-point 
positioning.  The  resulting  optimal  control  problem  is  highly  nonlinear  and 
constrained  due  to  the  nonlinearities  in  the  robotic  arm  dynamics  and 
kinodynamic  constraints  including  limits  on  joint  velocities  and  actuator  torques. 

This  thesis  illustrates  the  applicability  of  pseudospectral  methods  to  solve 
the  optimal  path  planning  problem  for  a  system  of  multi-link,  multi-degree  of 
freedom  robotic  arms.  The  optimal  control  problem  is  defined  in  standard  form 
and  solved  using  the  software  package  DIDO.  Pontryagin’s  Minimum  Principle  is 
used  to  verify  that  the  proposed  solution  satisfies  the  necessary  conditions  for 
optimality.  A  particularly  challenging  aspect  that  is  explored  is  the  optimal  motion 
of  multiple  arms  conducting  independent  tasks  with  the  risk  of  collision.  Collision 
avoidance  can  be  achieved  by  modeling  appropriate  path  constraints. 

The  processes  for  optimal  trajectory  planning  are  developed  for  a  single 
two  degree-of-freedom  manipulator  conducting  point-to-point  positioning  and 
extended  to  include  dual  three  degree-of-freedom  manipulator  maneuvers 
employing  collision  avoidance.  The  results  demonstrate  the  suitability  of 
pseudospectral  techniques  to  solving  the  minimum  time  and  minimum  control 
maneuvers  for  robotic  arms.  The  employment  of  collision  avoidance  techniques 
will  facilitate  continued  research  in  autonomous  robotic  motion  planning  using 
optimal  control  criteria  in  multiple  arm  systems. 
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I.  INTRODUCTION 


A.  MOTIVATION 

The  use  of  robotic  arms  has  become  commonplace  in  today’s 
technological  society.  These  machines  accomplish  tasks  from  the  mundane  to 
the  miraculous  with  little  regard  to  how  they  calculate  their  motion  by  the  majority 
of  the  populace,  after  all  the  motion  control  of  one’s  arm  is  much  more 
complicated  than  a  simple  robotic  arm  yet  it  is  accomplished  with  little  effort.  The 
reality  is  that  the  mathematical  model  for  robotic  motion  is  highly  nonlinear  and 
not  intuitive. 

Optimal  path  planning  is  the  primary  means  to  achieve  efficient  control  of 
robotic  manipulators  in  physical  space.  Methods  of  achieving  optimal  control 
have  been  researched  extensively  over  the  past  half  century,  but  it  is  only  with 
the  increase  in  computational  power  and  advanced  numerical  techniques  over 
the  past  decade  or  so  that  optimal  control  can  now  be  realized  in  a  wide  variety 
of  tasks.  The  field  of  robotics  has  historically  used  some  form  of  optimization  to 
achieve  efficiencies  in  the  motion  of  systems  with  the  goal  of  autonomous 
operations  implied  in  many  endeavors. 

Terrestrial  applications  of  robots  abound  and  the  economic  benefit  for 
achieving  efficiencies  in  motion  cannot  be  taken  lightly  and  is  a  driving  force 
behind  many  of  the  previous  studies  in  robotic  optimal  control  [1,2,3],  Even  a 
small  increase  in  efficiency  can  have  a  large  effect  on  throughput.  Some  of 
these  applications  require  a  form  of  autonomous  or  adaptive  path  planning,  and 
the  vast  majority  of  them  are  very  specific  tasks  that  are  suited  to  specific 
methods  of  calculating  highly  accurate  trajectories  that  satisfy  specific  conditions. 
Space  applications  of  robotic  manipulators  also  lend  themselves  towards 
exploring  autonomous  path  planning  for  a  variety  of  machines.  Robot  arms  have 
been  used  extensively  on  the  Space  Shuttle  and  the  International  Space  Station 
(ISS)  with  increasing  levels  of  complexity.  A  very  good  example  of  the  use  of 
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cooperative  robotic  arms  was  in  April  2001  when  the  ISS’s  Canadarm2  was  used 
to  transfer  cargo  directly  with  the  shuttle  Endeavor’s  arm  (Figure  1,  [4]).  While 
most  of  this  work  was  accomplished  via  manual  control,  only  a  little  imagination 
is  necessary  to  see  the  usefulness  of  autonomous  control  for  a  system  of 
cooperating  robotic  manipulators.  The  Naval  Research  Laboratory  (NRL)  has 
been  conducting  research  in  participation  with  the  Defense  Advanced  Research 
Project  Agency  (DARPA)  on  a  program  to  use  a  system  of  robotic  arms  on  an 
advanced  serving  satellite  [5],  SUMO,  or  Spacecraft  for  the  Universal 
Modification  of  Orbits,  is  a  program  that  is  being  investigated  to  integrate 
autonomous  rendezvous  and  grappling  technologies  into  a  relatively  low  cost 
means  of  altering  a  satellite’s  orbit.  As  Figure  2  shows,  SUMO  is  envisioned  to 
have  three  seven  degree-of-freedom  (DOF)  robotic  arms  that  will  require 
advanced  motion  planning  in  a  cluttered  environment  to  operate  safely.  The  NRL 
is  developing  control  algorithms  that  operate  in  real  time  and  must  satisfy  the 
non-linear  dynamics  and  constraints  of  the  system.  Traditional  optimal  control 
techniques  in  the  presence  of  obstacles  were  deemed  “unsuitable  for  real-time” 
use  due  to  their  speed  and  complexity. 


Figure  1 .  Cooperating  Robotic  Arms  in  Space:  International  Space  Station 
Arm  Canadarm2  Transfers  Cargo  to  Endeavor’s  Arm  April  28,  2001  (From  [4]) 
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Figure  2.  SUMO  Advanced  Servicing  Spacecraft  (From  [5]) 


B.  TRAJECTORY  PLANNING 

Traditionally,  the  study  of  robotic  control  has  been  approached  in  two 
stages  or  levels  (see  Figure  3).  The  first  stage  is  off-line  path  planning  or 
trajectory  planning.  Various  methods  are  utilized  to  compute  a  suitable  state 
trajectory  for  the  robot  to  achieve  the  desired  task  given  the  physical  constraints 
on  the  system.  Once  that  path  has  been  defined,  an  inner  loop  controller  can  be 
used  in  real  time  to  correct  the  errors  in  the  path  of  the  robot’s  actual  trajectory 
within  the  tolerance  value  of  the  desired  path  [6,7,8],  Most  previous  work  has 
been  focused  on  the  path  planning  problem  and  defining  some  form  of  an 
optimal,  collision  free  trajectory.  Ideally,  the  computation  of  the  optimal  control 
trajectory  could  take  place  within  a  controller  and  allow  a  single  stage  control  of 
robotic  systems.  However,  computation  time  required  for  the  path  planning 
problem  typically  makes  this  solution  infeasible  to  implement  in  real  time.  Figure 
3  diagrams  a  number  of  robotic  control  methods  and  illustrates  the  two-level 
approach  described  [3,  8], 
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Figure  3.  Synoptic  Diagram  of  Optimal  Motion  Planner  for  Robotic 

Manipulators  (From  [8]). 


The  problem  of  trajectory  planning  can  be  formulated  as  a  general  optimal 
control  problem  that  seeks  to  minimize  some  cost  function  (also  called  an 
objective  function  in  some  literature).  Given  the  state  variables  xeDWxand 
u  eD  Nu  ;  the  standard  optimal  control  problem  is  written  in  the  form: 

Minimize  J[x(),u(),ff  ]  =  E(x(tf  ),tf  )  + jF(x(),u(),f)dt 

to 

Subectto  x(f)  =  f  (x(f),u(0,f) 

x(f0)  =  x0  (1) 

eL(tf)<e(x(tf),tf)<eu(tf) 

where  J  is  the  cost  function,  E  is  the  endpoint  cost,  F  is  the  running  cost,  f  is  the 
set  of  equations  describing  the  dynamics  of  the  system,  e  is  the  set  of  endpoint 
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constraints,  and  h  is  the  set  of  path  constraints  [9],  The  most  common 
optimization  problem  in  robotics  is  to  solve  the  minimum  time  problem  where  the 
cost  function  E=tf  .  A  running  cost  such  as  F=uTu  is  also  used  to  reduce  the 
control  effort.  Variations  on  this  basic  cost  function  have  been  explored  in  the 
literature  [2,8,10,11,12,13],  The  endpoint  constraints  define  the  final  state  of  the 
system.  The  path  constraints  can  include  torque  and  velocity  limits  on  the  joints 
of  the  manipulator,  geometric  limits  to  the  system,  criteria  for  obstacle  avoidance, 
and  path  tracking  depending  on  the  task.  A  trajectory  is  considered  feasible, 
though  not  necessarily  optimal,  if  the  dynamics,  endpoint  constraints,  and  path 
constraints  are  satisfied. 

Trajectory  planning  for  robotics  typically  takes  one  of  two  forms:  a 
decoupled  approach,  which  first  finds  a  feasible  path  and  then  optimizes  the 
control  along  that  path;  and  a  direct  trajectory  planning  approach,  which  includes 
taking  kinodynamic  constraints  of  the  system  to  solve  an  optimal  path  [7,  8], 
These  two  methods  each  have  merits  and  drawbacks. 

A  substantial  amount  of  research  has  been  focused  on  decoupled 
methods  by  improving  the  performance  of  a  robotic  system  given  a  specified 
collision-free  path  [6,7,8,13,14,15],  These  types  of  trajectories  are  particularly 
useful  in  tasks  that  require  specific  trajectories  to  accomplish  the  assigned  task. 
As  an  example,  [13]  presents  an  algorithm  that  searches  for  a  time  optimal 
control  trajectory  while  minimizing  the  control  effort  along  a  specified  path.  This 
formulation  is  fairly  typical  of  previous  methods  and  useful  as  a  brief  illustration. 
For  a  robotic  manipulator,  the  specified  path  is  normally  defined  by  the  position 
and  orientation  of  the  tool  being  used.  The  dynamics  of  the  system  can  be 
derived  using  a  number  of  methods  such  as  the  Lagrange-Euler  method.  The 
path  is  first  specified  in  terms  of  the  generalized  joint  coordinates  and  then 
written  in  parametric  form  using  a  standard  interpolation  method.  The  method  of 
parameterizing  the  motion  of  the  robot  is  complex  and  not  trivial.  The  path  is 
scaled  and  its  first,  second,  and  third  order  derivatives  are  calculated.  The  cost 
function  is  then  restated  in  terms  of  the  scaled  parametric  values  and  minimized 
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in  terms  of  the  parametric  variables,  which  are  compared  to  the  kinematic 
constraints  of  the  system  to  check  for  feasibility.  Reference  [13]  uses  a  genetic 
algorithm  to  search  for  the  optimal  solution,  while  reference  [10]  proposes  phase 
plane  techniques  that  solve  the  minimum  time  problem  along  a  desired  path 
given  in  parametric  form.  Such  methods  often  assume  some  form  of  bang-bang 
control  near  the  limits  of  the  feasible  joint  space,  which  may  not  be  desirable 
since  the  sudden  change  in  manipulator  torques  can  cause  damage,  or 
excessive  wear,  or  excite  flexible  modes. 

When  the  path  is  not  specified,  approaches  have  been  proposed  to  solve 
the  global  time  optimal  solution  using  grid  or  cell  type  iterations  based  on  the 
decoupled  path  planning  problem  method  [16,  17].  These  are  highly  involved 
and  can  only  be  considered  to  converge  to  near-optimal  trajectories  in  some 
cases  [6],  In  general,  these  approaches  attempt  to  optimize  every  possible  path 
and  are  therefore  not  practical  in  high  dimensional  problems. 

Robotic  manipulators  usually  consist  of  multiple  links  and  can  achieve  an 
objective  maneuver  through  multiple  paths.  Optimization  results  often  produce 
surprising  results  that  can  be  physically  explained,  but  may  differ  from  an  intuitive 
solution  [3],  Full  motion  planning  or  direct  trajectory  planning  computes  the 
optimal  solution  in  the  state  space  of  the  system  and  solves  an  unknown  optimal 
trajectory  of  each  joint.  When  the  movement  of  the  robot  is  not  specified,  these 
trajectory  methods  are  more  useful  than  the  decoupled  path  planning  algorithms 
at  minimizing  some  cost  function  over  the  semi-infinite  range  of  possible 
trajectories.  Many  direct  trajectory  planning  techniques  have  been  explored  over 
the  past  three  decades.  Excellent  surveys  can  be  found  in  [6],  [8],  and  [19], 
Figure  4  is  a  useful  diagram  that  summarizes  some  of  the  methods  used  to  solve 
the  trajectory  planning  problems.  These  algorithms  can  be  used  to  solve  point- 
to-point,  or  pick-and-place  trajectory  problems  where  only  the  initial  and  final 
positions  of  the  end  effector  are  defined.  Path  constrained  optimization  problems 
can  also  be  solved.  Methods  of  computing  optimal  trajectories  generally  fall 
within  two  categories:  direct  and  indirect  [8], 
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Figure  4.  Optimal  Free  Trajectory  Planning  Problem  Methods  (From  [8]) 


Indirect  methods  are  primarily  based  on  Pontryagin’s  Minimum  Principle 
and  solving  the  necessary  conditions  for  optimality  [18,19],  Given  a  standard 
problem  formulation  of  the  form  of  Equation  (1),  the  control  Hamiltonian  is 
defined  where/-/  e  □  and  A  e  □  Nx  is  the  adjoint  covector  or  costate  vector: 

H(K,x,u,t).=  F(x,u,0  +  Arf(x,u,f)  (2) 

Then  applying  the  Lagrange  multiplier,  p  eD  Wh  in  order  to  define  the  Lagrangian 
of  the  Hamiltonian  yields: 

H(p,A,x,u,f)  =  H  +  prh  (3) 
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Similarly,  an  end-point  Lagrangian  is  given  by 

E  =  E  +  vre 


(4) 


where  veOWe.  The  lower  Hamiltonian,  J^is  the  value  of  the  Hamiltonian  with 
the  optimum  control  profile  and  is  defined  as 

^T(A,x,f)=  min^H(A,x,u,f)  (5) 


The  necessary  conditions  can  then  be  described  by  Equations  (6)-(10)  below  [9], 

dH 

Hamiltonian  Minimization  Condition:  —  =  0  (6) 

<9u 

Adjoint  Condition:  -A  = —  (7) 

dx 


Transversal ity  Condition: 


Hamiltonian  Value  Condition: 


A  (f0)  = 


A  (tf)  = 


dE 

dx(t0) 

dE 

dx(tf) 


(8) 


H(t0)  = 


H(tf)  = 


dE_ 

dt0 

_dE_ 

dtf 


(9) 


Hamiltonian  Evolution  Condition:  M  =  —  (10) 

dt 


The  optimal  trajectory  can  then  be  found  by  solving  the  multi-point 
boundary  value  problem  by  a  multiple  shooting  method  though  convergence  to  a 
solution  can  be  difficult  to  predict  [8],  An  excellent  survey  of  indirect  methods  for 
computing  the  optimal  trajectory  for  robotic  manipulators  is  presented  in  [19], 
The  penalty  for  applying  indirect  methods  to  solve  the  optimal  control  problem  is 
the  amount  of  effort  required  to  deriving  the  necessary  conditions,  particularly  for 
complex  systems. 
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The  most  common  method  to  solving  optimal  control  problems  in  recent 
years  is  by  using  direct  methods  [6,8,15,19],  In  general  these  methods  depend 
on  discretizing  the  state  and  control  variables  and  using  nonlinear  optimization 
techniques  [15],  evolutionary  or  hierarchical  approaches  [20,21],  or  stochastic 
methods  to  optimize  the  solution.  The  latter  two  techniques  tend  to  suffer  from 
“numerical  explosion  when  treating  high  dimension  problems  [8].” 

Almost  all  the  methods  to  solve  optimal  problems  require  a  level  of 
discretizing  the  state  and  control  parameters  in  some  way.  Traditional 
parameterization  is  done  by  uniformly  distributing  nodes  along  the  time  profile 
and  solving  a  constrained  optimization  problem  using  gradient  based  nonlinear 
optimization  techniques  such  as  sequential  quadratic  programming  (SQP).  An  in 
depth  approach  to  treating  the  optimal  control  problem  via  a  direct  means  is 
presented  in  [3]  and  is  representative  of  nonlinear  optimization  techniques.  An 
initial  control  function  is  used  to  provide  an  initial  guess  to  the  algorithm  within 
the  feasible  bounds  of  u.  The  corresponding  state  values,  x,  are  then 
approximated  using  a  collocation  technique  that  results  in  a  piece-wise  trajectory 
that  satisfies  the  system  dynamics  and  constraints  via  a  multiple  shooting 
method.  The  state  and  control  function  can  then  be  parameterized  and  used  to 
define  the  cost  function.  This  results  in  a  large  scale  nonlinear  optimization 
problem.  A  numerical  algorithm,  such  as  SQP,  which  is  tailored  to  the  structure 
of  the  problem,  is  then  used. 

Regardless  of  the  techniques  used  to  solve  the  path  planning  problem,  the 
direct  and  indirect  methods  can  be  used  to  complement  each  other.  A  candidate 
solution  can  be  verified  by  comparing  the  results  to  the  necessary  conditions  for 
optimality  [22],  A  hybrid  solution  is  presented  in  [2]  that  uses  a  direct  collocation 
approach,  which  parameterizes  the  state,  and  control  variables  with  a  poor  initial 
guess  trajectory  using  the  endpoints  to  interpolate  a  discreet  solution.  This  is 
then  used  to  approximate  a  nonlinear  optimization  problem  using  SQP. 
Pontryagin’s  necessary  conditions  are  then  calculated  symbolically  based  on  the 
dynamics  and  constraints  of  the  system.  The  results  of  the  direct  collocation 
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calculation  can  then  be  used  as  the  initial  guess  to  solve  the  multipoint  boundary 
value  problem.  While  this  method  is  useful,  the  calculation  of  the  necessary 
conditions  are  computationally  intensive  and  make  solving  anything  more 
complex  than  a  3-DOF  system  difficult  [2], 

Pseudospectral  methods  have  also  been  proposed  to  solve  the  trajectory 
planning  problem  for  robotic  manipulators  [23],  This  method  differs  from 
previous  direct  methods  primarily  by  the  discretization  method.  The  Legendre 
pseudospectral  method  approximates  the  state  and  control  variables  using 
particular  interpolating  polynomials.  The  discretized  nodes  are  non-uniformly 
spaced  based  on  Legendre-Gauss-Lobatto  point  allocation.  The  Covector 
Mapping  Theorem  allows  this  method  to  “make  no  distinction  between  the  so- 
called  direct  and  indirect  methods”  [22]  and  lends  itself  to  verification  by 
application  of  Pontryagin’s  Minimum  Principle  with  the  automatic  generation  of 
states,  costates,  and  other  dual  variables. 

C.  THE  COOPERATIVE  PLANNING  PROBLEM 

Multiple  robots  working  in  the  same  space  complicate  the  path-planning 
problem  exponentially.  There  are  generally  two  traditional  approaches  to 
attacking  the  problem  of  coordinated  movement  between  multiple  robots: 
centralized  planning  and  decoupled  planning  [6],  The  latter  consists  of  finding 
feasible  paths  for  each  robot  without  regard  to  the  other  robots,  in  other  words 
disregarding  any  risk  of  collision.  The  robots  are  then  sequenced  by  adjusting 
start  times  and  velocities  of  the  individual  arms  to  avoid  potential  collisions 
[24,25],  This  method  may  be  considered  attractive  from  a  computational  aspect 
because  the  dimensions  of  the  problem  are  limited  to  a  single  robot  at  a  time; 
however,  the  system  is  not  optimized.  The  former  robots  become  solid  obstacles 
in  the  way  of  the  latter’s  motion  and  may  shrink  their  feasible  work  space  to  zero. 
Cooperation  between  the  robots  is  unlikely. 

Centralized  planning  computes  the  trajectory  of  all  elements  of  the  system 
simultaneously  and  effectively  treats  the  system  as  a  single  state  vector.  As  the 
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number  of  individual  robots  increase,  the  dimension  of  the  problem  increases 
accordingly.  When  the  centralized  planning  problem  is  solved,  the  solution 
provides  the  state  and  control  trajectory  for  the  complete  system.  While 
computation  speed  may  seem  a  concern  in  implementing  the  method  on  practical 
applications,  increased  computing  power  and  efficient  numerical  techniques  are 
allowing  centralized  planning  of  more  and  more  complex  systems  to  be  plausible. 

Obstacle  avoidance  algorithms  rely  on  measuring  the  distance  between 
the  objects  and  including  some  buffer.  A  number  of  methods  that  maneuver  a 
point  target  around  an  obstacle-rich  environment  have  been  previously  proposed 
[26,27],  Distance  functions  are  used  in  most  algorithms  that  use  geometric 
shapes  to  define  the  obstacles,  most  of  which  compare  the  set  of  points  for  one 
obstacle  to  the  set  of  points  in  the  other  obstacle  and  finding  the  minimum 
distance.  This  requires  some  means  of  discretizing  the  obstacles  and  using  a 
brute  force  method  to  compare  each  point  [6,28],  For  example  using  a  finite 
number  of  different  radii  spheres  to  cover  the  obstacles  and  constrain  the 
minimum  distance  between  spheres  [8],  This  formulation  would  define  a  path 
constraint  such  that  at  each  point  in  time  the  minimum  distance  between  the 
centers  of  each  sphere  is  greater  than  some  reference  distance. 

A  variation  of  this  technique  was  presented  in  [29]  where  a  distance 
function  from  a  set  of  reference  points  of  each  manipulator  to  the  obstacle  was 
computed. 
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where  Dk  is  the  distance,  M'k  is  a  set  of  1=  1,...,n  points  that  describe  the  contour 
of  the  manipulator  arms  at  time  k,  P'k  is  a  set  of  points  that  describe  the  center  of 

each  obstacle,  i,  at  time  k,  and  Lj  describes  the  obstacle  size  in  each  direction. 
The  cost  function  can  then  include  the  term  Jo  where  J°D  is  a  reference  distance: 
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A  collision,  in  theory,  would  make  JD=0  which  is  the  maximum  value  in  this 
formulation.  A  collision  free  path,  assuming  the  function  is  properly  scaled, 
would  have  a  lower  cost  than  a  path  with  a  collision.  However,  there  is  no 
guarantee  that  the  system  will  avoid  a  collision. 


D.  A  COOPERATIVE  CONTROL  PROPOSAL 

Optimal  cooperative  path  planning  consists  of  simultaneously  solving 
trajectories  for  multiple  robotic  arms  while  meeting  all  obstacle  avoidance  criteria. 
Rather  than  discretizing  the  links  of  an  arm  and  solving  the  avoidance  problem  in 
the  work  space,  this  thesis  proposes  reformulating  the  minimum  distance 
problem  in  a  parametric  form  and  solving  a  parameterized  optimization  problem. 
Pseudospectral  optimal  control  formulations  readily  lend  itself  to  solving  such 
problems. 

Using  the  software  program  DIDO,  pseudospectral  methods  are  used  to 
solve  the  optimal  minimum-time  trajectory  for  2-DOF  and  3-DOF  robotic 
manipulators  conducting  point-to-point  maneuvers  where  the  initial  point  is  given 
in  joint  coordinates  and  the  final  point  in  Cartesian  coordinates.  The  computed 
state,  costate,  and  control  trajectories  facilitate  verifying  Pontryagin’s  necessary 
conditions  for  optimality.  The  calculated  control  trajectory  is  then  propagated  to 
demonstrate  feasibility.  Building  on  these  results,  dual  manipulator  solutions  are 
analyzed  and  presented  using  variations  of  cost  functions.  Finally,  two  methods 
of  formulating  obstacle  avoidance  criteria  are  presented  and  compared  in  various 
scenarios.  A  static  obstacle  avoidance  formulation  is  first  illustrated  followed  by 
a  system  of  two  multiple  link  arms  performing  a  simple  pick-and-place  operation. 


12 


II.  OPTIMAL  TRAJECTORY  PLANNING 


Pseudospectral  (PS)  methods  have  been  used  to  solve  a  variety  of 
trajectory  planning  and  optimal  control  problems  as  shown  in  [27,30,31,32,33], 
PS  methods  have  been  flight  tested  onboard  the  International  Space  Station,  and 
other  flight  experiments  are  in  the  planning  stages.  Given  their  widespread 
applicability,  it  should  be  no  surprise  that  the  manipulator  path  planning  problem 
is  a  logical  application  of  these  techniques.  Motion  planning  for  time-optimal 
point-to-point  maneuvers  in  a  pick-and-place  operation  is  considered  here.  The 
optimal  control  formulation  incorporates  realistic  constraints  on  the  joint  velocities 
and  accelerations  as  well  as  bounds  on  actuator  torque  to  ensure  the  solution 
trajectories  are  physically  realizable.  The  efficacy  of  PS  optimal  control 
techniques  is  demonstrated  via  simulation.  The  first  analysis  considers  a  simple 
two  link,  two  degree  of  freedom  (2-DOF),  three-dimensional  motion  of  an  arm 
with  a  full  development  of  kinematics,  dynamics,  constraints,  and  Pontryagin’s 
necessary  conditions  [18].  The  second  analysis  uses  the  same  principles  to 
develop  trajectories  for  a  single  three  link,  3-DOF  manipulator. 

A.  PROBLEM  FORMULATION  FOR  A  2-DOF  MANIPULATOR 

The  robotic  arms  used  throughout  this  study  are  based  on  the  Cyton 
Alpha  7D1G  from  Robai.  While  not  exact,  the  model  assumes  homogenous  rigid 
arms  and  perfect  actuators.  Dynamics  that  are  more  complex  can  be 
incorporated  into  the  problem  formulation  with  the  same  techniques 
demonstrated  below  used  to  formulate  the  optimal  control  problem. 

1.  Modeling  Dynamics  and  Kinematics 

The  Cyton  Alpha  is  a  seven  degrees  of  freedom  manipulator  with  an  end 
effector  as  pictured  in  Figure  5  [34],  Specific  joints  can  be  locked  to  simulate 
fewer  degrees  of  freedom.  The  first  problem  formulated  is  a  three  dimensional 
motion  of  a  2-DOF  arm  as  sketched  in  Figure  6  where  a-,  is  the  offset  in  the  local 
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x  coordinate  system  and  di  is  the  offset  in  the  local  z  direction.  0!  is  the  angle  of 
rotation  of  the  base  and  02  is  the  angle  of  the  arm  with  respect  to  horizontal. 
Based  on  the  Cyton  Alpha  specifications,  limits  were  placed  on  the  motor  torque 
for  each  joint,  Timax  and  the  maximum  angular  velocity,  u)imax.  Initial  discrepancies 
between  the  model  dynamics  and  the  actual  arm  in  the  laboratory  required 
angular  acceleration  limits,  aimax  in  order  to  increase  the  accuracy  of  the  results. 
Link  1,  the  base  unit,  is  modeled  as  a  homogeneous  cylinder  and  the  arm  is 
modeled  as  a  thin  rod. 


Figure  5.  Cyton  Alpha  7D1G  7-DOF  Robotic  Manipulator  (From  [34]) 


Figure  6.  Sketch  of  2-DOF  Robotic  Arm  Model 
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Link  1  (Base) 

Link  2  (Arm) 

a-i  0.00  m 

di  0.15  m 

mi  0.150  kg 

Tlmax  2.4  N  m 

tOimax  7.5  rad/sec 

aimax  7.5  rad/sec2 

a2  0.48  m 

d2  0.02  m 

m2  0.385  kg 

T2max  2.9  N  m 

(jL)2max  7.5  rad/sec 

c<2max  1 0  rad/sec2 

Table  1 .  2-DOF  Dimensions  and  Limits 

It  is  usually  desirable  to  map  the  position  of  the  end  effector  from  joint 
coordinates  to  a  Cartesian  coordinate  system.  The  origin  for  the  coordinate 
system  is  the  center  of  the  base.  A  Denavit-Hartenberg  homogenous  transfer 
matrix  was  used  to  characterize  the  kinematics  of  the  robotic  arm.  Using  planar 
rotations  and  linear  displacements  Equations  (11)  and  (12)  explicitly  define  the 
end  effector  position. 

cos#,  -sin#,  0]  f (  a1  )  l~cos#2  0  -sin#2Va2V 

sin#,  cos#,  0  -d2  +0  1  0  0  (11) 

0  0  1  [y  c/,  J  sin#2  0  cos#2  [  0  Jy 

xe  =  a,  cos  #,  +d2  sin  #,  +  a2  cos  #,  cos  #2 
ye  =a,  sin^  -d2cos#2  +a2  sin#, cos#2  (12) 

ze  =  at,  +  a2  sin  #2 

While  this  is  the  method  employed  almost  universally  and  will  be  used 
throughout  this  work,  other  means  of  deriving  the  kinematics  are  available  and 
may  prove  to  be  better  suited  to  solving  the  optimal  control  problem.  Reference 
[15]  has  suggested  the  use  of  Lie  groups  and  Lie  algebras  to  formulate  the 
kinematics  and  subsequent  dynamics  of  the  system. 

The  dynamics  of  the  robotic  arm  were  calculated  using  a  Lagrangian 
formulation  of  the  equations  of  motion  [7,35,36],  The  general  equation  of  motion 
takes  the  form 

D(q)q  +  C(q,q)q  +  g(q)  =  Q  (13) 
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where  q  is  the  vector  of  generalized  coordinates  (0i  and  02  in  this  case),  D  is  a 
positive  definite  inertia  matrix,  C  is  the  velocity  coupling  vector,  g  is  the 
gravitation  force  vector,  and  Q  is  the  generalized  force  vector  or  the  joint  torques, 
ti  and  x2  in  this  case.  The  algorithm  presented  in  [35]  was  used  to  develop  the 
general  equations  for  this  model  symbolically  using  the  software  program  Maple 
and  is  presented  in  the  appendix.  Using  the  values  from  Figure  6,  Equation  (13) 
was  solved  and  is  shown  to  three  significant  figures  for  the  2-DOF  three- 
dimensional  model  in  Equation  (14). 


"0.0128  +  0.0126  cos202 

-O.OO183sin202 

-0.001 83  sin  202 

0.0291 

V^2  J 

-0.02910,4  sin202  -  0.001 8342  cos02 
0.01464  sin 202 


0 

0.899  cos  02 


f  -r-  \ 


\T2  J 


(14) 


2.  Formulation  of  Optimal  Control  Problem 

The  dynamics  formulated  above  are  the  basis  for  the  optimal  control 
problem  formulation  shown  in  Equation  set  (15).  Let  x  be  the  state  vector  and  u 
the  control  vector.  The  cost  function,  J,  was  chosen  to  minimize  the  time  of  the 
maneuver  based  on  the  constraints  of  the  system.  Other  cost  functions  can  be 
developed  to  minimize  the  control  effort  (such  as  a  quadratic  cost),  energy,  or 
some  combination  of  elements,  e  is  the  endpoint  function  where  the  initial  angles 
6 f  and  the  final  endpoint  coordinates  [xf,  /,  zf]  are  given  by  solving  Equation  (12) 
for  a  feasible  6^  and  02.  In  addition,  let  h  be  defined  as  the  path  constraints  on 
the  function  that  in  this  case  is  simply  the  angle,  angular  velocity,  and  torque 
limits. 
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X  = 


~e; 

'  6,  :-90°  <0,  <90° 

e2 

eX:=< 

d2 :45°  <02  <135° 

4 

4  :  -7.5  <6*  <  7.5 

4. 

4  :-7.5<4  <7.5 

eU:= 


f  T.\ :  -2.4  <z^<  2.4 j 
jr2  :-2.9<r2  <2.9{ 


Minimize  J[x(r),u(r),ff  ]  =  tf 


Subject  to:  x(f)  = 


*1 

4 

a 


*(U  = 


A 

4 

4 

o 

o 


CO, 


C0o 


a. 


a,, 


where  «/  =  f(x>u) 


e(xntf)  = 


a1  cos  +  d2  sin  6,  +  a2  cos  cos  #2  -  xf 
a.,  sin  ^  -  d2  cos  02  +  a2  sin  cos  d2  -  y f 
c/1  +a2  sin  6*2  -zf 


<z>. 


(15) 


[0] 


ivi 

rxul 

A 

<  h(x,u)< 

A 

.  nU 

u 

u 

3.  Necessary  Conditions 

To  be  optimal,  the  solution  must  satisfy  Pontryagin’s  necessary  conditions. 
Define  the  Hamiltonian,  H  as  a  function  of  running  cost,  F,  the  vector  of  costates, 
A(t),  and  the  right-hand  side  of  the  dynamics,  x(f)  =  f(x,tv,f): 

/-/( A, x, u, f )  :=  F  +  Arf  ;  A  e  □  Wx 

H  —  Aq  (Oy  +  Ag^  co2  +  A^  cr1  +  A^  cc2  0  6 ) 

The  path  constraints  must  be  taken  into  account  as  well.  For  the  simplest 
2  DOF  problem  the  constraints  are  the  limits  of  the  control  and  state  vectors 
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defined  in  Equation  set  (1515).  Equation  (17)  defines  the  Lagrangian  of  the 
Hamiltonian,  H . 


H:=H  +  [iT  h; 


H 


□  Nh 


h  =  n0  e ,  +  fie  e2  +  (4  +  «  )«,  +  (4  +  4  H  +  4, +  a  «2  +  4  r,  +  4  r. 


(17) 


Similarly,  the  Equation  (18)  defines  the  Endpoint  Lagrangian,  E  where  E 
is  the  endpoint  cost: 

E{t0,tf,x0,xf,w):=E  +  vre;  veON' 

E  =  <x0  +  44  +  44  +  uzfzf  +  v> 


Pontryagin’s  necessary  conditions  are  defined  in  Equations  (19)  -  (23). 

dH 

The  Hamiltonian  Minimization  Condition  (HMC)  requires  - —  =  0 

9u 


dH_ 

du 


"an' 

dr, 

dH 

VdT2) 

V 

day  da2 

4 - ^  4, - r  uT 

a<x,  da2 

4, - 1-  4 1 - r  UT 

ar,  ar,  r= 


dH_ 

au 


T  14600  491 7  sin  0, 

+  ^ - -  +  jU 

Den  Den  ri 

4  91 7  sin  #2  4  (7390 +  7280  cos  202) 

'  +  ~ - = - +  4 


Den 


Den 


=  [0] 


Den  =  214  +  212cos2<92  +1  ,68cos2  02 


(19) 


The  adjoint  condition  requires  -A 


dH 

dx 
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.  da,  da2 

A - 1“  A - h  Lin 

80,  c°2  80 '  1 

.  5^  0a2 

A - h  A - h  Lin 

®1  a/3  fa2  a/3  rtJ2 


, .  ..  da,  .  da2 

{k*^)+K'~s^AK^ 

(X,  +„  )+3  ^l  +  3  ^ 

V  02  ^  ^  «*  0(y2 


(4,  +  /4, ) + 4 


•  ••+  A 


(K2  +  )  +  4 


/  -i  \  '■'“•I  /  i  \  '-/LA2 

(  T  +  //  ) - h  (  A  +  //  ) - b  //  „ 

sin(2#2)(252 co2  -15.9sin(6,2)<«1) 

^  127  +  126  cos(26,2  )  +  cos2  02  +” 

sin(26'2)(15.9sin(6'2)«2  -128<a,  -126^  cos2<92) 
1 27  +  126  cos(26>2  )  +  cos2  6>22 
sin(26'2)(252«2  -15.9sin(6'2)(«1) 

^  127  +  126  cos(26i2  )  +  cos2  #22  +” 

+  ,  sin(6'2)(15.9sin(26'2)«1  +  2cos(<92)&>2) 
“*  127  +  126  cos(26i2  )  +  cos2  02 


It  is  valuable  to  note  that  A01  is  constant  and  can  be  used  to  verify  the 
optimality  conditions.  The  transversality  condition  requires 

A  =  Ht0)  =  ~ 


A(f0)  =  -vx 


K(tf)=  »xf 


dxf  8yf  dzf 

°xf~d0,+°xy~d0,+Vzf~d0, 
dxf  £yf  8zf 

°xfW2+°xy~d0'2+Vzf~d0'2 
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uxf  (-0.476  sin  6,  cos62  +O.O2cos01)  +  --- 

•••  +  t;xy(0.476cos61  cos62  +  0.02sin6>) 
t>xf  (-0.476  sin  6>  sin62)  +  --- 

■■■  +  oxy  (-0.476  sin  6,  sin  d2 )  +  vzf  (0.476  sin  02 ) 


The  Hamiltonian  Value  Condition  requires: 

H(t,)  =  ~  =  ~ 1  (22) 

dtf 

Finally,  the  Hamiltonian  Evolution  Equation  requires  that  the  optimal 
control  trajectory  satisfies  Equation  (23). 

H  =  —  =  0  (23) 

dt 

The  above  necessary  conditions  provide  two  useful  checks  for  optimality. 
The  Hamiltonian  can  be  analyzed  for  optimality  and  should  be  a  constant  -1  for 
all  time  and  A0i  is  constant. 

4.  Minimum  Time  Simulation  Set-up  and  Results 

The  three-dimensional  2-DOF  robotic  arm  is  used  to  demonstrate  the 

applicability  of  using  pseudospectral  techniques  to  solve  the  time-optimal 

trajectories  with  various  endpoint  conditions  using  the  software  package  DIDO. 

Two  trajectories  are  presented  below.  To  ensure  that  the  endpoint  coordinates 

are  reachable  within  the  state  bounds,  feasible  initial  and  final  angles  were  first 

selected.  The  final  angles  were  then  mapped  to  the  corresponding  final 

manipulator  endpoints  using  Equation  (12).  The  assumption  in  this  the  simplest 

case  is  that  the  final  orientation  of  the  manipulator  is  not  a  constraint  on  the 

system,  only  that  it  is  at  the  correct  position.  It  is  important  to  note  than  even  in 

the  2-DOF  system,  a  given  set  of  joint  angles  produces  a  unique  point  in  space, 

but  the  reverse  is  not  necessarily  true.  A  point  in  space  may  have  multiple 
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solutions  in  joint  space  and  choosing  specific  final  angles  to  ensure  the  feasibility 
of  the  maneuver  does  not  guarantee  that  those  angles  would  be  the  optimal  final 
state. 

To  obtain  the  optimal  solution,  DIDO  was  run  in  normal  mode  using  a 
bootstrapping  technique.  The  initial  run  was  calculated  using  a  16  node  solution 
that  in  turn  was  used  as  a  guess  for  a  30  node  solution  that  in  turn  was  used  as 
an  initial  guess  for  a  60  node  solution.  It  was  found  that  60  nodes  were  sufficient 
to  properly  propagate  the  system  using  a  differential  solver  such  as  ODE45  and 
using  the  linearly  interpolated  values  of  the  discrete  control  trajectory. 

Table  2  lists  the  initial  conditions  used  for  two  scenarios.  The  final  angles 
were  used  to  calculate  the  final  endpoint  constraints.  Scenario  1  consisted  of  a 
positive  90°  rotation  about  the  base  of  the  arm  with  the  initial  and  final  angle  of 
the  second  link  at  45°.  Figure  7  shows  the  optimal  trajectory  results  of  that  run. 
As  shown,  the  time-optimal  maneuver  was  calculated  to  be  a  simple  rotation 
about  the  base.  Figure  7  -  Figure  9  plot  the  results  for  analysis.  As  shown,  H=-1 
and  Aei  is  constant  as  required  by  Equations  (23)  and  (20).  The  minimum  time 
maneuver  completes  in  0.92  seconds  for  this  problem  formulation. 


2-DOF  Simulation  Scenario  1 

2-DOF  Simulation  Scenario  2 

0°  =0 

Ol  =  45° 

O  Lfi 

ii  ii 

T  ^CM 

xf  =0.020 
yf  =0.337 
zf  =0.483 

LO 

o  ^r 

ii  ii 

q^ 

O  o 

(J>  io 

i  ^r 

ii  ii 

<3T 

xf  =-0.020 
yf  =-0.337 
zf  =0.483 

Table  2.  2-DOF  Simulation  Initial  Conditions 


21 


Angular  Rates  [rad/s]  Angles  [rad] 


((<=0 

(,(=45 


Xf  =  0.020  m 

yf  =  0.337  m 
zf=  0.483  m 


Figure  7.  2  DOF  arm  simulation,  Scenario  1:  90°  maneuver. 
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Figure  9.  2-DOF  Arm  Simulation,  Scenario  1:  Costates  and  Hamiltonian 


Scenario  2  was  designed  as  a  similar  maneuver  with  the  arm  rotating  -90° 
about  the  base.  Again,  the  final  coordinates  of  the  manipulator  end  effector  were 
calculated  using  Equation  (11).  The  results  were  quite  different  than  the  previous 
scenario  as  shown  in  Figure  10  with  a  maneuver  time  of  0.880  seconds.  Figure 
11  shows  that  the  final  base  angle,  0i,  is  not  -90°  as  expected  but  rather  83.2° 
(1.45  radians)  and  the  arm  elevation,  02,  is  135°.  No  assumptions  were  made  as 
to  the  trajectory  of  the  arm  and  it  became  obvious  that  the  endpoint  coordinates 
could  be  achieved  in  two  ways  and  solving  the  reverse  kinematics  problem 
shows  this.  The  pseudospectral  solver  solved  the  solution  that  required  the 
minimum  time  to  accomplish,  which  in  this  case  was  a  rotation  of  83.2°  vice  a 
rotation  of  90°  due  to  the  offset  of  the  arm  from  the  centerline  of  the  base. 


23 


Angular  Rates  [rad/s]  Angles  [rad] 
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Figure  10.  2-DOF  Arm  Simulation,  Scenario  2:  “-90°”  maneuver. 
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The  control  torques  on  both  joint  actuators  were  indirectly  constrained  in 
the  simulation  by  an  added  constraint  on  the  angular  acceleration  based  on 
empirical  performance  characteristics  measured  on  a  physical  arm. 
Nevertheless,  the  pseudospectral  solver  produced  a  control  that  resembles  a 
bang-bang  control  for  both  actuators  as  shown  in  Figure  12.  However,  the 
torque  required  to  rotate  the  base  was  noticeably  smaller.  The  momentum  of  the 
arm  in  combination  with  the  small  base  torque,  was  sufficient  to  rotate  the  arm 
the  83.2°  required  to  complete  the  maneuver. 


time 


Figure  12.  2-DOF  Arm  Simulation,  Scenario  2:  Control  Torque 


To  ensure  the  feasibility  of  the  trajectory,  the  control  torque  was 
interpolated  using  a  basic  linear  interpolation  function  in  Matlab  and  propagated 
using  the  same  dynamic  equations  (14)  and  the  ODE45  function  at  20  Hz 
intervals  (the  minimum  sampling  rate  of  the  arm).  The  results  are  plotted  in 
Figure  13  with  the  propagated  angles  and  angular  velocities  overlaid  every  0.05 
seconds  on  the  DIDO  solution. 
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Figure  13.  2  DOF  Arm  Simulation,  Scenario  2:  Propagated  State  Vector. 

While  relatively  simple  dynamics  were  used  in  the  above  problem 
formulation,  the  method  can  be  used  on  more  complex  dynamics  that  include 
actuator  characteristics  and  non-rigid  links  [37],  The  PS  methods  implemented 
by  DIDO  would  refine  the  optimal  solution  as  the  accuracy  of  the  model 
increases. 

B.  PROBLEM  FORMULATION  FOR  3-DOF  MANIPULATOR 

1.  Modeling  Dynamics  and  Kinematics 

The  Cyton  Alpha  arm  was  also  modeled  as  a  3-DOF  manipulator.  Figure 
14  is  a  sketch  of  the  configuration  used  in  deriving  the  equations  of  motion.  The 
angle  of  the  bottom  link  was  locked  at  45°.  Maple  was  used  to  map  the  joint 
positions  and  end-effector  position  from  link  coordinates  to  Cartesian  coordinates 
similar  to  the  2-DOF  case  above.  Equation  (24)  defines  the  positions  of  the  arm 
end-effector  as  functions  of  0j.  For  the  purposes  of  this  simulation,  the  two  free 
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links  have  dimensions  given  in  Table  3  and  are  joined  by  simple  revolute  joints 
with  the  angular  limits  defined  by  Equation  set  (25).  The  Equations  of  motion  for 
this  model  were  developed  using  the  algorithm  presented  in  [35]  in  form  of 
Equation  (13)  using  Maple. 


Figure  14.  3  DOF  Robotic  Arm  Configuration 


Link  1  (Base+Arm  1) 

Link  2 

Link  3 

ai  0.115m 

di  0.270  m 

mi  0.375  kg 

a2  0.170  m 

d2  0.020  m 

m2  0.100  kg 

a3  0.140  m 

d3  0.010  m 

m3  0.110  kg 

Table  3.  3-DOF  Arm  Dimensions 


xe  =  a1  cos  0,  +  (d2  +  d3 ) sin  0,  +  a2  cos  cos  02  +  a3  cos  cos(02  +  9Z ) 
ye  =  a1  sin ~(c/2  +d3)cos01  +a2  sin#,  cos#2  +  a3  sin6>  cos(6>2  +  6>3 )  (24) 

ze  =  (3f1  +  a2  sin  02  +  a3  sin(6'2  +  <93 ) 
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2. 


Formulation  of  Optimal  Control  Problem  and  Necessary 
Conditions 


The  optimal  control  problem  builds  on  the  previous  2-DOF  example.  With 
the  state  and  control  variables,  x  and  u  respectively,  defined  below,  the  minimum 
time  problem  formulation  is  shown  in  Equation  set  (25).  The  0,  and  r,  limits  were 
defined  in  the  Cyton  documentation.  In  order  for  the  links  to  be  kept  rigid  by 
intermediate  motors,  angular  acceleration  limits,  Oi  were  imposed  on  the  system 
based  on  actual  tests  conducted  on  the  arm.  It  is  important  to  note  that  Qj  =  «,  in 

the  inertial  reference  frame.  While  not  ideal,  the  above  described  formulation  is 
to  demonstrate  the  feasibility  of  the  pseudospectral  methods  and  an  exact  model 
was  deemed  beyond  the  scope  of  this  investigation. 
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6*3 :  -85°  <  6*3  <  85° 

>  u  = 

’ril 

z7 

e(i:=< 

't,  :-2.4<t,  <2.4 
:  -2.9  <  r,  <  2.9  > 

4 

4 

4J 

6,  :-7.5<^  <7.5 

4  :  -7.5  <02<  7.5 
4  :  -5.8  <  4  -  5-8 

_r3_ 

z  z 

r3  :  -0.5  <  r3  <  0.5 

Min 


j[xO,u(~),tf  =, 


Subject  to:  x  = 


1 

_ 1 

1 

_ 1 

4 

4 

cox 

al 

( b , 

a, 

1 

1 _ 

a3 

x(to)  =  [01° 


4  oi 


where  a,  =  /(x,u) 
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(25) 
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e(x,tf)  = 


x„  -  x 


ye  ~y 


z„  -  Z- 
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ox 


[0] 


VI 

VI 

-10  <al  <10 

u1 

<h(x,u)< 

uu 

where 

-7.5  <a2<  7.5 

aL\ 

°v 

-7.5  <  a3  <  7.5 

Just  as  in  the  2-DOF  problem,  Pontryagin’s  necessary  conditions  for 
optimality  were  derived  for  the  3-DOF  arm.  These  conditions  are  similar  in  form 
to  Equations  (16)  -  (23).  The  only  two  useful  results  of  the  necessary  condition 
analysis  are  that  the  H=- 1  for  all  time,  t  and  Aei  is  constant. 


3.  Simulation  Results 

A  number  of  simulations  using  this  problem  formulation  were  conducted 
using  DIDO.  For  each  simulation,  the  initial  angles  and  the  final  manipulator 
coordinates  were  defined.  The  final  position  of  the  robotic  end-effector  was 
chosen  such  that  the  final  joint  angles  were  at  their  upper  limits.  Figure  15 
shows  the  results.  Just  as  they  were  calculated  in  the  2-DOF  model,  the 
manipulator  coordinates  were  calculated  using  Equation  (24)  given  a  set  of 
feasible  joint  angles.  The  results  are  similar  to  those  from  the  2-DOF  problem. 
Figure  16  and  Figure  17  show  the  computed  state  and  control  trajectories  for  this 
problem. 
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X7  =  0.03 
yf  =-0.11 
z7  =  0.32 


Figure  15.  Single  Arm  3  Degree  of  Freedom  (3-DOF)  Optimal  Time  Maneuver 
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Figure  16.  Single  Arm  3-DOF  Simulation  State  Trajectory 
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Figure  17.  Single  Arm  3-DOF  Simulation  Control  Trajectory 

The  increase  in  complexity  from  the  2-DOF  to  3-DOF  problem  is  evident 
from  the  time  duration  of  the  pseudospectral  calculations.  The  runtime  for  the  2- 
DOF  problem  averaged  a  few  seconds,  while  the  3-DOF  problem  took  about  one 
minute.  More  importantly  was  that  DIDO  required  an  initial  guess  for  some  of  the 
endpoint  conditions  in  order  to  find  a  feasible  solution.  For  this  problem,  a  two 
point  guess  was  used  with  the  initial  and  final  angles  and  a  final  time  of  2 
seconds.  The  angular  velocity  and  control  torque  were  assumed  to  be  0  for  the 
guess.  DIDO  was  able  to  solve  the  optimal  control  problem  and  find  a  feasible 
solution.  Future  versions  of  DIDO  will  incorporate  more  advanced  guess-free 
algorithms  such  as  those  discussed  in  [38]  that  may  alleviate  this  requirement. 

C.  DUAL  ARM  TRAJECTORY  PLANNING 

The  ultimate  goal  of  this  investigation  is  the  simultaneous  path  planning  of 
multiple  robotic  manipulators.  As  such,  the  next  step  taken  was  to  formulate  the 
minimum  time  problem  using  two  arms  and  analyze  the  results.  Below  is  the  3- 
DOF  problem  formulation  with  simulation  results  from  both  2-DOF  and  3-DOF 
arms.  No  attempt  was  made  to  avoid  arm  interference,  but  only  to  demonstrate 
the  ability  for  pseudospectral  methods  to  calculate  dual  arm  trajectories  and 
compare  to  single  arm  trajectories. 
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1. 


Problem  Formulation 


The  dynamics  and  endpoints  of  the  multi-arm  problem  are  formulated 
similarly  to  the  single  arm  problem.  The  dual  arm  formulation  is  presented  below 
for  the  3-DOF  robotic  motion  case.  In  this  case,  the  arms  are  identical  copies  of 
each  other,  but  this  does  not  need  to  be  the  case.  Equation  Set  (26)  defines  the 
endpoint  coordinates  for  each  arm  a  and  b.  Arm  b  is  offset  from  arm  a  and  the 
origin  by  the  vector  [xb,yb ,zb]T  . 


xae  =  a1  cos 9a  +  (d2  +  d3 ) sin  6*  +  a2  cos 9a  cos 92  +  a3  cos 0a  cos{92  +  03 ) 
yae  =  a1  sin  9a  -  (d2  +  d3 ) cos 9a  +  a2  sin  9a  cos 0a  +  a3  sin  9a  cos{0a  +  0a ) 
zae  =d,+a2  sin  0a  +  a3  sin(6»2a  +  6a ) 

xbe  =  x3  +  a1  cos 6b  +{d2+d3) sin 6b  +  a2  cos 6b  cos 02  +  a3  cos 0b  cos (02  +  03  ) 
Ye  =y o  +  ai  s'n  -  (d2  +  d3 )  cos  6b  +  a2  sin  9b  cos  02  +  a3  sin  6b  cos(02  +  03 ) 
zbe  =  c/1  +  a2  sin  9b  +  a3  sin(6>2  +  9b ) 


Equation  sets  (27)  and  (28)  show  that  the  dynamics  of  the  problem  are 
completely  uncoupled  in  its  formulation. 
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(27) 


The  optimal  control  problem  is  now  defined  as  follows: 
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Minimize 


J[x(n),u(n),ff]  =  ff 


Subject  to:  x(f)  = 
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(28) 


2. 


Simulation  Results 


Adding  the  second  arm  increases  the  complexity  of  the  problem  and  for 
some  endpoint  conditions,  a  simple  two-point  guess  was  required  for  DIDO  to 
converge  to  a  solution.  A  bootstrapping  technique  was  used  with  a  16  node 
solution  being  calculated  then  used  as  a  guess  for  a  30  node  and  then  a  60  node 
solution.  The  second  arm  was  offset  from  the  origin  by  30  cm  in  the  -y  direction. 
Figure  18  plots  the  simulation  results  for  the  two  arms  with  the  given  endpoint 
conditions.  Figure  19  plots  the  Hamiltonian  for  the  system  and  verifies  the 
necessary  Hamiltonian  Value  Condition. 
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Figure  18.  Dual  Arm  3-DOF  Simulation  (No  Obstacle  Avoidance) 
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Figure  19.  Dual  Arm  3-DOF  (No  Obstacle  Avoidance)  Hamiltonian 


— ,  A 

■A/  > 

These  results  were  derived  using  minimum  time  as  the  cost  function. 

While  all  the  joint  angles  are  coupled,  generally  there  is  a  joint  that  limits  the 

minimum  time,  in  this  case  62  for  arm  A  and  6\  for  arm  B.  In  addition,  one  arm 

will  limit  the  minimum  time  of  the  entire  multi-arm  system.  It  is  important  to  note 

that  the  minimum  time  for  the  system  to  complete  the  maneuver  is  dictated  by 
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the  arm  whose  time-optimal  maneuver  takes  longer  to  complete.  The  trajectory 
of  the  other  arms  and  joints  will  technically  meet  the  minimum  time  condition 
regardless  of  the  path  it  takes  so  long  as  it  arrives  at  its  endpoint  conditions  at  tf. 

Figure  20  plots  the  differences  that  can  occur  between  the  dual  arm  and 
single  arm  minimum  time  maneuvers.  The  solid  lines  in  the  figures  are  the  single 
arm  solutions  and  the  dashed  lines  are  from  the  dual  arm  system.  The  minimum 
time  defining  joint  6>?  for  arm  A  is  nearly  identical  for  both  the  single  and  dual  arm 
algorithms.  The  other  joints,  while  not  following  exactly  the  same  trajectory,  did 
find  the  same  minimum  time  route  to  the  endpoint.  Arm  B,  which  took  0.92 
seconds  to  complete  its  maneuver  in  the  single  arm  computation  follows  a  similar 
path  in  the  dual  arm  algorithm,  but  at  a  slower  rate. 


Arm  A  angles  Arm  B  angles 


Figure  20.  Comparison  of  Dual  Arm  Algorithm  and  Single  Arm  Algorithm 

While  technically  the  results  are  time-optimal,  there  is  no  measure  of 
efficiency  in  the  computation  of  the  angles  of  arms  that  are  not  limited  by  the 
system  minimum  time.  A  good  example  is  the  oscillations  in  arm  B’s  6\  and  02  in 

the  dual  arm  algorithm.  One  remedy  is  to  solve  for  the  optimal  trajectory  in  two 
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steps.  First,  solve  the  minimum  time  problem,  then  use  that  minimum  time  to 
bound  the  horizon  of  the  problem  and  solve  a  minimum  energy  or  quadratic 
problem  among  the  various  possible  minimum  time  trajectories.  Another  solution 
is  to  add  some  measurement  of  efficiency  such  as  a  minimum  energy  or 
minimum  control  to  the  cost  function  to  find  a  minimum  time  solution  that  also 
maximizes  the  efficiency  of  the  system.  The  modified  cost  function  could  take 
the  form  of  Equation  (29)  where  [A]  is  a  diagonal  matrix  of  weighting  factors. 
Assuming  tf  is  sufficiently  large  compared  to  the  running  cost,  the  minimum  time 
solution  can  be  found  while  simultaneously  maximizing  the  efficiency  of  the 
solution. 

tf 

J  =  ff +JiT[A]u  (29) 

0 

The  results  of  rerunning  the  same  endpoint  conditions  using  Equation  (29) 
as  the  cost  function  with  unity  weighting  are  presented  in  Figure  21 .  A  number  of 
conclusions  can  be  made  from  this  plot.  First,  it  illustrates  that  there  are  multiple 
minimum  time  paths,  particularly  for  those  joints  that  do  not  limit  the  minimum 
time.  The  final  time  using  the  new  cost  function  is  the  same  to  within  10'3  second 
while  decreasing  quadratic  control  cost  by  26.3%.  This  improvement  factor  was 
found  by  numerically  integrating  the  calculated  control  vector  of  the  two 
scenarios.  In  addition,  these  results  demonstrate  the  effect  of  gravity  on  the 
system.  In  Figure  20,  arm  B’s  second  and  third  links  remain  nearly  parallel  as  02 
and  03  remain  at  nearly  0  throughout  the  maneuver.  However,  including  the 
quadratic  cost  demonstrates  that  the  minimum  control  torque  required  to 
complete  the  maneuver  is  not  the  parallel  motion.  02  and  03  in  Figure  21  actually 
drop  below  parallel  to  minimize  the  total  control  torque  over  the  course  of  the 
maneuver. 
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Time  [sec]  Time  [sec] 

Figure  21 .  Comparison  of  Dual  Arm  Algorithm  and  Single  Arm  Algorithms 

Including  Minimum  Effort 


Changing  the  cost  function,  J,  to  Equation  (29),  does  alter  some  of  the 
necessary  conditions,  but  does  not  change  the  two  useful  conditions  used  in  this 
analysis  to  check  for  optimality:  H( t)=-1  and  A01=constant.  What  it  also  does  not 
do  is  simultaneously  find  minimum  time  solution  for  the  arm  that  does  not  limit 
the  minimum  time  for  the  system.  A  viable  solution  for  this  using  pseudospectral 
methods  is  not  presented  here  but  poses  an  interesting  path  for  future  research. 


D.  AN  ALTERNATE  PROBLEM  FORMULATION 

While  the  standard  problem  formulation  used  above  finds  an  optimal 
solution,  there  are  other  ways  of  formulating  this  optimization  problem.  One  such 
alternate  method  that  was  investigated  was  to  reformulate  the  problem  statement 
by  eliminating  all  trigonometric  functions.  Rather  than  defining  the  state  variables 
in  terms  of  0i  and  Wi,  let  the  state  variables  be  defined  by  the  sine  and  cosine  of 
the  angles.  While  this  increases  the  dimension  of  the  state  variables,  there  are 
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no  trigonometric  functions  in  the  dynamics  which  may  be  computationally 
advantageous  .  The  state  x,  control  u  and  dynamics  x  for  each  arm  are  defined 
for  the  2-DOF  problem  as 


fV 

'  ^  ' 

“4^0, 
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<J)2  C(>2 
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Because  the  states  are  no  longer  angles,  the  following  additional  path 
constraints  are  required  on  the  system: 

C\  +SJ  -1  =  0 
C]  +Sl  -1  =  0 

c/2  C/2 

The  bounds  on  the  states  based  on  the  physical  limitations  of  the  system  must 
also  be  reformulated: 


0  <  C„  <  1 

*1 
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The  endpoints  must  also  be  redefined  by  the  new  state  variables: 


"  sin  6^'  " 

cos  6\ 
sin  6^ 

v  cos  &2  y 
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x, (c;,s;,c;,s;) 

y, (c;.s;,c;,s;) 

z, (c;,s;,c;,s;) 


u)  =  =  0 


Using  the  same  endpoint  conditions  in  section  2-A,  the  alternate  problem 
formulation  was  solved  using  Equation  (29)  as  the  cost  function  in  DIDO. 
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Regardless  of  cost  function,  tf=  0.92  seconds  which  corresponds  to  the  minimum 
time  solution  of  the  +90°  rotation  from  scenario  1  above.  Figure  22  illustrates  the 
calculated  system  trajectory. 


Figure  22.  2  Arm,  2-DOF  Simulation  Using  Alternate  Problem  Formulation 


This  result  shows  that  other  proper  problem  formulations  will  result  in  the 
same  trajectory  as  the  original  problem  formulation.  Figure  23  compares  the 
calculated  joint  angle  trajectories  for  the  two  problem  formulations.  It  is  apparent 
that  regardless  of  the  problem  formulation,  DIDO  computed  a  very  similar 
solution.  However,  there  are  preferred  formulations  for  numerical  computation 
efficiency.  Figure  24  demonstrares  that  the  original  problem  formulation,  for  this 
particular  model,  is  better  scaled  and  can  be  solved  an  order  of  magnitude  faster. 
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Figure  23.  Comparison  of  Original  and  Alternate  Problem  Formulation 
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Figure  24.  30  Node  Hamiltonian  Values  for  Different  Problem  Formulations 
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III.  OBSTACLE  AVOIDANCE 


A.  BACKGROUND 

Of  major  importance  in  robotic  trajectory  planning  is  solving  the  obstacle 
avoidance  problem.  In  the  most  simplistic  terms,  the  issue  is  one  of  determining 
the  distance  between  each  arm  and  any  potential  obstacle.  A  number  of  studies 
have  been  published  on  obstacle  avoidance  in  motion  planning  of  robotic 
vehicles  [1,  12,  28  ,39,  40]  where  the  obstacle  is  mathematically  modeled  as  an 
enclosed  polygon  with  the  vehicle  as  a  point  in  space  and  time.  A  path  constraint 
that  restricts  the  point  from  entering  the  polygon  by  a  sufficient  buffer  ensures 
collision  avoidance  is  included  in  the  algorithm.  While  this  is  useful  in  a  number 
of  autonomous  motion  planning  problems,  it  does  not  account  for  a  set  of 
continuous  points  such  as  arms  of  finite  length.  A  different  path  constraint 
formulation  is  required. 

In  the  most  general  sense,  each  manipulator  link  and  obstacle  can  be 
modeled  as  some  rigid  geometric  shape  and  the  minimum  distance  between 
each  link  and  each  potential  collision  must  be  calculated  at  each  point  in  time. 
Solving  the  obstacle  avoidance  problem  for  a  continuous  set  of  points  rather  than 
a  discrete  point  in  space  and  time  complicates  the  problem.  In  the  simplest  case, 
each  link  of  each  arm  can  be  modeled  as  a  line  segment.  The  minimum  distance 
between  every  two  links  that  have  the  potential  to  collide  must  be  computed  at 
each  time  and  incorporated  into  the  path  constraint.  The  optimal  control  problem 
increases  in  complexity,  but  pseudospectral  methods  can  still  be  used  to  solve 
the  trajectory  with  this  path  constraint  and  a  given  minimum  distance  between 
links. 


1.  Minimum  Distance  between  Two  Continuous  Lines 

Before  discussing  the  obstacle  avoidance  algorithm,  it  is  necessary  to  first 
determine  the  minimum  distance  between  two  static  arms.  Each  link  is  modeled 
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as  a  line  segment  using  the  parametric  equations  of  a  line  in  □  3 .  Define  p1  and 
p2  as  the  two  endpoints  of  an  individual  line  segment: 


These  values  are  then  used  to  define  the  following  values  for  lines  a  and  b: 

a0=pl  a  =  Pa  -Pi 

b°=p i  and  b  =  P'-Pi 


The  equations  for  two  links  modeled  as  a  line  segment  where  t  and  s  are 
parametric  variables  take  the  form: 


a  =  a0+af  te[0,1] 
b  =  b0+bs  s  e  [0,1] 


(30) 


For  any  two  values  for  t  and  s,  the  distance  between  the  vectors  a  and  b  is  the 
norm  of  the  vector  difference  between  them. 


d  = 


a-b 


[(a0+af)- 


■  (b0  +  b s)]T  [(a0  +  at)  -  (b0  +  bs)] 


(31) 


It  is  more  convenient  to  calculate  the  square  of  the  distance.  Squaring  Equation 
(31 )  and  grouping  terms  yields: 

d 2  =  aTat2  +2(aTa0  -aTb0)t -2aTbst +  ■■■ 

(32) 

•••+  2(brb0  -b7"a0js+  b7bs2  +aga0  -2agb0  +bgb0 
The  coefficients  of  Equation  (32)  are  scalar  values.  Let 
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A  =  aTa 

B  =  2(ara0-a7b0) 

C  =  2aTb 

D  =  2(brb0  -b7a0) 

E  =  bTb 

F  —  aQaQ  —  2a0b0  +  b0b0 


(33) 


Substituting  Equation  set  (33)  into  (32)  gives 

d2  =  At2  +Bt-Cst  +  Ds  +  Es2  +F  (34) 

Equation  (34)  is  a  parabaloid  in  □  3  with  A  >  0  and  E  >  0-  The  minimum 
distance  for  two  infinite  lines  is  found  by  calculating  the  minimum  distance  over 
all  points  f.seO  .  This  is  an  unconstrained  optimization  problem  with  the  cost 
function  Y  =  d2  and  the  optimization  variable  x  =  [  t,  s  ]T  or 

Minimize  Y(t,s)  =  At2  +  Bt -Cst  +  Ds  + Es2  +  F  (35) 

xeD  2 

The  minimum  must  satisfy  both  the  stationary  condition  and  convexity  condition: 

av  _g  aJY 
ax  x=x-  ax2 


>o 

x=x* 


where  x*  are  the  parametric  coordinates  of  the  minimum  distance  between  the 
two  lines.  The  stationary  condition  for  Equation  (35)  is 
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and  solving  for  x*  yields 


2 AD+BC  .2 BE+CD 

(C2 -4AE)  ~  (C2  -4 AE) 


(36) 


(37) 
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Likewise,  the  convexity  condition  takes  the  form 


'  d2Y  d2Y 
d2Y  _  ~W  dtds 
5x2  d2Y  d2Y 
_dsdt  ds2 

Equation  (38)  is  semi-positive  definite  and  meets  the  convexity  condition 
when  4AE-  C2  >0.  This  is  true  for  all  t  and  s  and  can  be  seen  by  breaking  it  into 
the  vector  components: 

4  AE-C2  =4(ara)(b7b)-4(arb)(a7b) 

=  4  |a|2  |b|2 -(|a||b|cosi9)2  >0 

It  is  interesting  to  note  that  a  unique  solution  only  exists  when  4AE-  C2^0. 
When  4AE-C2=0,  the  two  arms  are  parallel  and  there  is  an  infinite  set  of 
solutions.  Substituting  Equation  (37)  into  (35)  gives  the  minimum  distance 
between  two  infinite  lines. 

2.  Minimum  Distance  between  Two  Line  Segments 

While  the  discussion  on  the  minimum  distance  between  infinite  lines  is 
useful  to  this  problem,  the  minimum  distance  between  two  arm  segments  is  a 
box-constrained  optimization  problem: 

min  Y{t,s)  =  At2  +  Bt -Cst  +  Ds  + Es2  +F 
subj  to  0  <  t  <  1  (39) 

0  <  s  <  1 

The  Lagrangian  of  Equation  (39)  is  now  defined  as 

Y(x,  A)  =  At2  +  Bt  -  Cst  +  Ds  +  Es2  +  F  +  Tft+Tss 
and  the  first  order  necessary  condition  is 
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(40) 


This  is  a  more  complicated  problem  to  solve.  The  parametric  coordinates 
for  the  minimum  distance  are  now  functions  of  the  Lagrange  multipliers  At  and  As. 

,  2EB  +  CD  +  2EAt  +  C/L  ,  2AD  +  CB  +  C1 +2AA 

LiHd2  (c2-4AE)  lmind2  (C2  -4/4E) 


The  Lagrange  multipliers  in  Equation  (40)  and  (41)  must  be  found  and 
satisfy  the  Karush-Kuhn-Tucker  (KKT)  conditions: 
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<0 

t=  0 

<0 

s  =  0 

=  0  for 

0  <  f  <  1  and 

V 

=  0  for 

0  <  s  <  1 

>0 

t  =1 

>0 

s  =  1 

(42) 


Aj  cannot  be  solved  analytically  in  a  general  sense  and  other  methods  must  be 
used  to  evaluate  the  endpoints  of  the  segment. 

References  [41]  and  [42]  develop  geometric  algorithms  to  solve  the 
minimum  distance  between  line  segments  which  are  summarized  below.  The 
first  step  is  to  calculate  4AE-  C2  to  find  out  if  the  line  segments  are  parallel.  Next, 
the  minimum  parameters  t*  and  s*  for  two  infinite  lines  that  are  defined  by 
extending  the  two  line  segments  are  calculated.  If  those  values  are  both 
between  0  and  1,  then  those  are  the  coordinates  of  the  minimum  distance  and 
can  be  used  to  calculate  d  Equation  (34).  If  t*  is  outside  the  range  [0,1]  and  s*  is 
within  the  range,  then  the  tmin  is  located  at  the  endpoint  closest  to  t*.  Using  that 
endpoint  condition  as  a  constant,  Equation  (36)  can  then  be  solved  for  smin.  If 
smin  is  now  outside  the  range  [0,1]  then  s  is  set  to  the  endpoint  closest  to  smin  and 
a  new  tmin  is  computed.  The  reverse  is  also  true.  This  approach  can  be  applied  if 
the  line  segments  are  parallel  and  if  both  global  minimums  are  outside  of  the 
range. 
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Figure  25  demonstrates  the  algorithm  that  can  be  applied  to  determine 
parametric  coordinates  that  define  the  minimum  distance  between  two  line 
segments  using  values  from  Equations  (33)  and  (37).  While  this  algorithm 
efficiently  calculates  the  minimum  distance  between  two  static  lines,  it  requires 
nine  if-then  statements  and  is  not  necessarily  a  continuously  differentiable 
function  over  time. 


Figure  25.  Geometric  Algorithm  for  Min.  Distance  Between  Line  Segments. 
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B.  STATIC  OBSTACLE  AVOIDANCE 


1.  2-DOF  Case 

The  above  algorithm  was  used  as  a  path  constraint  to  define  the  minimum 
distance  between  the  centerline  of  an  arm  and  the  centerline  of  a  cylinder 
defined  by  the  respective  endpoints.  DIDO  allows  the  path  constraint  to  be  a 
function  that  incorporates  the  complexity  of  the  geometric  analysis.  The  problem 
formulation  is  identical  to  Equation  set  (1515)  except  the  added  path  constraint 
c/7n  <  d2  is  included  where  d  is  the  instantaneous  distance  between  the  arm  and 

the  obstacle  and  dmin  is  the  pre-defined  obstacle  avoidance  distance.  For  the 
purposes  of  simulation,  dmin=  5  cm.  The  centerline  of  the  obstacle  was  placed  45 
cm  above  the  base  of  the  arm  and  oriented  along  the  y  axis  of  the  Cartesian 
coordinate  system.  Once  again,  an  initial  16  node  solution  was  calculated  and 
then  used  as  a  guess  for  a  more  refined  60  node  solution  in  order  to  interpolate  a 
feasible  control  trajectory  when  propagating  the  solution  using  ODE45  in  Matlab. 
The  relevant  initial  and  final  conditions  of  the  simulation  are  included  in  Table  4. 
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II 
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CD 
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0 

[Xef,  yef,  Zef]  =[.186,  .281,  .483] 

[xi,  yi,  Zilobs  =  [-0.5,  0,  .55] 

0 

LO 

II 

o 

Cfmin  —  5  Cm 

[X2,  y2,  z2]obs  =[  0.5,  0,  .55] 

Table  4.  Example  Endpoint  Conditions  for  Single  2-DOF  Arm  with  Static 

Obstacle  Avoidance 


Figure  26a  demonstrates  the  solution  for  the  defined  maneuver  without 
imposing  the  obstacle  avoidance  path  constraint  and  using  the  cost  function 

tf 

J  =  tf  +  .5ju7u 

o 

As  the  figure  illustrates,  the  arm  passes  through  the  centerline  of  the 
obstacle.  Figure  26b  demonstrates  the  same  maneuver  with  the  collision 
avoidance  algorithm  active.  The  trajectory  of  the  arm  changes  to  allow  a 
minimum  5  cm  buffer  between  the  arm  and  line.  Figure  27  -Figure  29  provide  the 
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corresponding  state  and  control  trajectories,  the  costate  values  over  time,  the 
Hamiltonian  value,  and  the  variation  of  the  minimum  distance  between  the  arm 
and  the  obstacle  during  the  obstacle  avoidance  maneuver. 
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Figure  27.  2-DOF  Motion  with  Static  Obstacle  Avoidance:  State  and  Control 

Trajectories 
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Figure  28.  2-DOF  Motion  with  Static  Obstacle  Avoidance:  Costate  and 

Flamiltonian  Trajectories 


Figure  29.  2-DOF  Motion  with  Static  Obstacle  Avoidance:  Distance  between 

Link  and  Obstacle  (5  cm  Buffer) 
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2. 


3-DOF  Case 


Similar  to  the  2-DOF  case,  the  3-DOF  problem  formulation  is  identical  to 
Equation  Set  (25)  with  the  added  complexity  of  the  path  constraint  imposed  to 
guarantee  a  minimum  distance  between  both  links  and  the  obstacle.  The  path 
constraint  for  this  problem  includes 

dLn  ^  < 

dLn^d2 

where  d\_mm  are  the  minimum  distances  between  the  obstacle  and  each 
respective  link.  The  joint  positions  must  be  computed  at  each  node  in  time  using 
the  kinematic  model  and  these  points  define  the  line  segments  used  to  compute 
the  minimum  distance. 


cd 

o 

II 

00 

O 

0 

[Xef,  yef,  Zef]  =  [.029,  -.334,  .049] 

[Xi,  yi,  zi]obs  =[-1,0,  0.4] 

0 

LO 

II 

o 

dlmin  —  0.05  m 

[X2,  y2,  z2]obs  =[1,0,  0.4] 

020  =0° 

d2min  =  0.05  m 

Table  5.  Example  Endpoint  Conditions  for  Single  3-DOF  Arm  with  Static 

Obstacle  Avoidance 


Figure  30a  demonstrates  the  minimum  time  solution  for  the  above 
maneuver  without  the  obstacle  avoidance  algorithm.  As  the  figure  illustrates,  the 
arm  passes  through  the  centerline  of  the  line  used  to  model  the  obstacle.  Figure 
30b  demonstrates  the  same  maneuver  with  the  collision  avoidance  algorithm 
active.  The  trajectory  of  the  arm  changes  to  allow  a  minimum  5  cm  buffer 
between  the  arm  and  static  cylinder.  The  maneuver  time  for  both  simulations 
was  1.22  seconds.  The  difference  in  the  trajectory  can  be  attributed  to  the 
weighted  quadratic  cost  function.  Figure  31  -  Figure  33  are  plots  of  the  solution 
trajectories.  Of  particular  interest  are  the  Hamiltonian  and  Aei  that  still  satisfy  the 
necessary  condition  derived  in  the  original  3-DOF  case. 
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[a]  [b] 

Figure  30.  3-DOF  Maneuver  Without  and  With  Static  Obstacle  Avoidance:  60 

Node  Solution 
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Figure  31 .  3-DOF  Motion  with  Static  Obstacle  Avoidance:  State  and  Control 

Trajectories 
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Figure  32.  3-DOF  Motion  with  Static  Obstacle  Avoidance:  Costates  and 

Hamiltonian 


Distance  Between  Link  A  and  Obstacle  Distance  Between  Link  B  and  Obstacle 


Figure  33.  3-DOF  Motion  with  Static  Obstacle  Avoidance:  Distance  between 

Links  and  Obstacle 


In  applying  the  PS  based  obstacle  avoidance  algorithm,  one  must  note 
that  the  numerical  solution,  uses  discrete  times  or  nodes  to  calculate  the  state 
and  control  trajectories  and  the  path  constraints  are  only  tested  and  guaranteed 
at  each  of  these  nodes.  A  low  node  solution,  while  feasible  at  each  discrete 
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node,  may  not  to  be  physically  realizable.  Figure  34  and  Figure  35  illustrate  a 
16-  node  solution  for  the  same  problem.  The  nodal  spacing  in  these  cases  is 
such  that  the  trajectory  seems  to  jump  the  obstacle,  that  is  the  path  constraints 
are  met  at  the  two  nodes  on  either  side  of  the  obstacle  as  shown  in  Figure  35  but 
the  arm  trajectory  would  collide  with  the  obstacle  as  it  proceeds  from  one  node  to 
the  next.  The  possibility  of  nodal  jumping  requires  that  the  computed  trajectory 
be  analyzed  to  ensure  it  is  feasible  and  may  necessitate  increasing  the  number 
of  nodes,  and  therefore,  the  computational  time  to  find  a  feasible  solution. 


Figure  34.  3-DOF  Motion  with  Static  Obstacle  Avoidance:  16  Node  Solution 


Distance  Between  Link  A  and  Obstacle  Distance  Between  Link  B  and  Obstacle 


Figure  35.  Minimum  Distance  Between  Links  and  Obstacle,  16  Node  Solution 
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c. 


NUMERICALLY  SOLVING  THE  KKT  CONDITIONS 


Returning  to  the  discussion  of  minimum  distance  between  line  segments, 
solving  Equation  (41)  should  produce  the  minimum  distance  parametric 
coordinates.  While  this  equation  cannot  be  solved  analytically,  it  can  be  solved 
numerically  using  DIDO. 

Taking  a  step  back  to  basic  optimality,  the  constraints  on  the  variables 
can  be  rewritten  in  the  form  y(x)  <  0  where  y  is  a  vector  of  constraint  functions. 

minimize  Y(t,s)  =  At2  +Bt-Cst  +  Ds  +  Es2  +F 
subj  to  -t<0  -s<  0 

f-1<0  s-1 <  0 

Taking  the  Lagrangian  of  the  cost  function  yields  a  new  optimization 
problem. 

minimize  Y  (x,K)  =  At2  +  Bt  -Cst  +  Ds  +  Es2  +F -Xnt  +  At2(t -1)-As1s  +ls2(s-1) 
subj  to  A>0 

Ary  =  0 


Rewriting  Equation  (40)  using  the  new  Y  gives 


dY 


dx 


Imin 


dY 

r 

dt 

>  =  < 

dY 

.  ds , 

2At  +  B-Cs-/ lM 
—Ct  +  D  +  2Es  — 


+  A 


1 2 


+  A 


's  2 


with  the  constraints 


4  (-0=0  4(-s)  =  o 

Ut- 1)  =  0  42(s-1)  =  0 


fo] 


0 


(43) 


(44) 


By  letting  s,  t,  and  X\  be  six  dummy  control  variables  for  each  possible 
collision  in  the  problem  formulation,  DIDO  can  numerically  solve  the  problem  with 
the  KKT  conditions  that  resemble  the  distance  between  two  lines.  Defining  the 
minimum  distance  allowed  between  two  arms,  Equations  (34),  (43)  and  (44) 


54 


specify  seven  path  constraints  that  are  associated  with  each  potential  collision. 
While  this  increases  the  number  of  variables  and  functions,  it  directly  solves  the 
original  obstacle  avoidance  problem  without  geometric  checks  and  resulting 
pitfalls  described  by  Figure  25. 


1.  2-DOF  Case 

The  motion  planning  problem  was  reformulated  to  include  the  dummy 
control  variables  and  additional  path  constraints  for  the  case  of  two  2-DOF  arms: 


u 


-  >1  - 


,  'To  ,  T*  ,  T 2  j  T' 


,t,S,  ,  /l^2  j  4l  ’  ^s2 


hL  <h[x(0), ii(D)]  <h“ 
d^m  <  At2  +  Bt-  Cts  +  Ds  +  Es2  +  F  <  oo 
0  <  2At  +  B  —  Cs  —  2fi  +  ^  0 

0  ^  —Ct  +  D  +  2 Es  —  +  As2  <  0 

o<(-OAi<o 

0  <  (f-1)/tf2  <  0 
0  <  (-s)Ts1  <  0 
0<(s-1)2s2<0 


Using  DIDO,  the  optimal  trajectory  was  computed  by  solving  the  distance 
between  the  arms  at  each  node  and  bounding  the  dummy  control  variables: 

0  <t  <1 

0  <  s  <1 

0  <  /l,  <  GO 

The  results  of  this  formulation  are  compared  to  those  found  using  the 
geometric  algorithm  and  presented  in  Figure  36  and  Figure  37.  The  state  and 
control  trajectories  are  nearly  identical  with  tf  and  cost  differences  are  on  the 
order  of  10'6.  The  only  significant  difference  was  the  computational  time.  On 
average,  the  KKT  algorithm  converged  to  a  solution  for  the  2-DOF  problems 
nearly  twice  as  fast  as  the  geometric  algorithm. 
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Figure  36.  2-DOF  Static  Obstacle  Avoidance:  State  and  Control  Trajectories 
(KKT  Algorithm  versus  Geometric  Algorithm) 
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Figure  37.  2-DOF  Static  Obstacle  Avoidance:  Hamiltonian  Values  (KKT 
Algorithm  versus  Geometric  Algorithm) 
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2. 


3-DOF  Case 


The  3-DOF  example  was  also  reformulated  using  the  KKT  algorithm  with 
an  18-element  control  vector,  u  and  14  path  constraint  equations.  Figure  38 
presents  the  results  of  this  simulation.  The  maneuver  time  for  both  the  geometric 
algorithm  and  the  KKT  algorithm  are  the  same  to  within  10'3  at  1.221  seconds. 
Both  functions  used  a  cost  function  of  the  form 

J  =  tf+.  ijiTu  (45) 

0 

The  difference  in  cost  between  the  two  algorithms  was  0.001  and  may 
account  for  the  slight  difference  in  x2  and  x3  trajectories.  The  Hamiltonian 
presented  in  Figure  39  corresponds  to  the  required  necessary  condition,  H=- 1, 
just  as  it  did  for  the  geometric  algorithm.  The  distance  between  the  obstacle  and 
the  two  links  of  the  arm  are  presented  in  Figure  40. 

The  only  major  difference  between  the  solutions  for  the  two  minimum 
distance  algorithms  is  the  computational  time.  It  is  difficult  to  accurately  assess 
the  runtime  using  DIDO  because  other  processes  are  often  running  on  the 
computer  and  solution  times  vary  considerably  for  the  same  problem.  Typically 
runtimes  for  a  16-node  solution  took  5-15  minutes  regardless  of  the  algorithm 
used.  In  an  effort  to  make  a  meaningful  comparison,  a  previous  60  node  solution 
was  used  as  an  initial  guess  for  each  problem  formulation.  A  60  node  solution 
took  115  seconds  to  run  for  the  KKT  algorithm  compared  to  93  seconds  for  the 
geometric  algorithm.  The  trend  appears  that  as  the  complexity  of  the  problem 
increases  and  the  dimension  of  control  vector  increases,  the  KKT  solution  is 
slower  than  the  Geometric  solution  for  the  same  problem. 
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Figure  38.  3-DOF  Static  Obstacle  Avoidance:  State  and  Control  Trajectories 
(KKT  Algorithm  versus  Geometric  Algorithm) 
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Figure  39.  3-DOF  Static  Obstacle  Avoidance:  Hamiltonian  Values  (KKT 
Algorithm  versus  Geometric  Algorithm) 
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Figure  40.  Minimum  Distance  Between  Links  and  Obstacle  (KKT  Algorithm 

versus  Geometric  Algorithm) 


59 


THIS  PAGE  INTENTIONALLY  LEFT  BLANK 


60 


IV.  COOPERATIVE  PLANNING  USING  PS  METHODS 


The  implementation  of  the  obstacle  avoidance  algorithm  in  Chapter  III 
demonstrates  that  pseudospectral  techniques  are  capable  of  solving  a  range  of 
problems.  While  the  model  was  specific  to  an  arm  and  a  static  cylinder-type 
object,  the  next  step  is  to  implement  the  same  concept  to  find  an  optimal  path  to 
more  complex  systems.  As  part  of  this  research,  one  interesting  problem  is  to 
solve  the  optimal  control  trajectory  of  a  multiple  arm  system  where  the  arm  links 
are  modeled  as  line  segments.  Path  planning  for  two  arms  has  already  been 
presented  in  Chapter  II.  Adding  the  path  constraints  from  the  obstacle  avoidance 
algorithms  presented  in  Chapter  III  allows  for  cooperative  or,  at  the  least, 
simultaneous  control  of  a  system  of  arms.  Two  examples  are  presented  below 
using  the  dual  arms  that  detail  optimal  path  planning  for  2-DOF  and  3-DOF 
motions. 

A.  DUAL  2-DOF  MANIPULATORS 

The  dual  2-DOF  arm  cooperative  planning  problem  is  a  straight  forward 
modification  of  the  static  obstacle  formulation.  The  obstacle  is  now  each  link  of 
the  other  arm.  The  minimum  distance  between  the  links  of  the  two  arms  can  be 
computed  for  any  given  time  using  either  of  the  algorithms  presented  in  Chapter 
III  and  can  be  constrained  to  be  greater  than  some  preset  distance.  Table  6 
presents  the  endpoint  conditions  used  to  define  the  problem. 


Arm  a 

Arm  b 

o  o 

CO  CO 

ii  ii 

o  o 

'St-  'S  CM 

xf  =0.337 

yf  =-0.02 

zf  =  0.483 

=  0° 

=  45° 

xf  =0.102 
yf  =0.084 
zf  =0.559 

Table  6.  2-DOF  Cooperative  Path  Planning  Endpoint  Conditions 
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The  two  arms  were  offset  by  0.30  m  along  the  y  axis  and  the  final 
coordinates  of  the  end-effector  were  chosen  based  on  angles  0i  =  0°  and  02=  45° 
for  Arm  a  and  0i  =  -60°  and  02  =  60°  for  Arm  b.  DIDO  was  used  to  solve  the 
problem  with  thecost  function  from  Equation  (45).  The  path  constraint  uses  the 
endpoints  of  the  arm  links  at  each  “node”  to  compute  the  distance  between  the 
arms  and  was  constrained  to  be  above  some  minimum  distance  c/min. 

The  optimal  maneuver  for  this  particular  setup  with  no  obstacle  avoidance 
algorithm  c/min=  0  has  a  closest  point  of  approach  of  10  cm  and  takes  0.75 
seconds  to  complete.  Figure  41  shows  such  unconstrained  optimal  state 
trajectory  and  Figure  42  is  the  minimum  distance  between  the  arms  throughout 
the  maneuver.  For  this  maneuver,  J=0.968. 

Arm  a  Arm  b 


Figure  41 .  2-DOF  Cooperative  Path  Planning  with  No  Obstacle  Avoidance: 

State  T rajectory 
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Figure  42.  2-DOF  Cooperative  Path  Planning  with  No  Obstacle  Avoidance: 

Distance  between  Arms 


Figure  43.  Cooperative  Path  Planning  for  Dual  2-DOF  Arms  with  19.5  cm 

Minimum  Clearance 
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If  10  cm  is  not  sufficient  for  the  arms  to  avoid  colliding  it  is  imperative  to 
modify  the  problem  and  define  a  more  restrictive  dmm.  For  the  maneuver  with  the 
above  endpoint  conditions,  the  maximum  buffer  distance  between  the  arm  links 
is  19.7  cm.  The  following  example  uses  the  same  endpoint  conditions;  however, 
dm\n  was  given  as  19.5  cm.  DIDO  was  run  using  both  the  geometric  and  KKT 
collision  avoidance  algorithms  with  similar  results.  Figure  43  depicts  the  arms  at 
four  points  along  the  computed  path.  A  red  line  is  attached  to  the  points  on  each 
arm  where  the  minimum  distance  between  the  arms  occur.  While  not  clear  in  the 
plots,  that  minimum  distance  line  is  perpendicular  to  both  arms.  Figure  44  - 
Figure  47  show  results  using  the  geometric  algorithm  for  obstacle  avoidance. 

Arm  1  Arm  2 


Figure  44.  2-DOF  Cooperative  Path  Planning  using  Geometric  Algorithm  and 

19.5  cm  Buffer:  State  Trajectories 
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Figure  45.  2-DOF  Cooperative  Path  Planning  using  Geometric  Algorithm  and 

19.5  cm  Buffer:  Control  Trajectories 


Figure  46.  2-DOF  Cooperative  Path  Planning  using  Geometric  Algorithm  and 

19.5  cm  Buffer:  Hamiltonian  Value 


Figure  47.  2-DOF  Cooperative  Path  Planning  using  Geometric  Algorithm  and 
19.5  cm  Buffer:  Minimum  Distance  between  Arms 
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Compared  to  the  case  with  no  obstacle  avoidance,  the  total  cost  of  the 
dynamic  optimization  problem  for  the  optimal  path  maneuver  including  a  19.5  cm 
buffer  was  found  to  be  J=1.067,  a  10%  increase.  The  maneuver  time  also 
increased  to  0.82  seconds  and  the  weighted  quadratic  cost  increased  by  .02 
units.  The  only  substantial  difference  between  the  KKT  algorithm  and  the 
geometric  algorithm  was  the  computational  time  to  converge  to  a  solution.  On 
average,  the  KKT  formulation  was  greater  than  twice  as  fast  as  the  geometric 
algorithm.  Using  a  60-node  solution  with  a  previously  computed  60-node  guess, 
the  KKT  algorithm  took  11.9  seconds  to  solve  versus  35  seconds  for  the 
geometric  algorithm. 

B.  DUAL  3-DOF  MANIPULATORS 

The  multiple  3-DOF  robotic  arm  obstacle  avoidance  problems  are  more 
complicated.  The  multiple  time-optimal  paths  for  each  arm  plus  the  nearly  infinite 
obstacle  avoidance  paths  make  this  an  interesting,  but  challenging  problem. 
While  the  algorithms  presented  to  this  point  are  theoretically  capable  of  solving 
the  problem,  the  number  of  potential  collisions  create  more  complex  path 
functions.  For  this  3-DOF,  dual  arm  system,  there  are  six  potential  collisions  that 
must  be  accounted  for  vice  one  in  the  relatively  simple  2-DOF  system  above.  In 
the  case  of  the  KKT  algorithm,  this  results  in  42  path  constraints  and  36  “dummy” 
control  variables  that  compute  the  distance  between  any  two  links.  Table  7 
summarizes  the  endpoint  constraints  used  for  this  simulation. 
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Table  7.  3-DOF  Cooperative  Path  Planning  Endpoint  Conditions 
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For  this  example,  0\  =  0Q  was  used  to  compute  the  final  endpoint  position 

in  Table  7  for  both  arms  to  ensure  the  feasibility  of  the  final  condition.  Arm  B  was 
placed  30  cm  from  Arm  A  in  the  -y  direction  with  the  same  3-DOF  limits  from 
Equation  set  (25).  As  was  presented  in  the  2-DOF  case,  Figure  48  and  Figure 
49  present  the  results  with  c/min=0.  The  closest  approach  occurred  between  the 
manipulator  links  of  the  two  arms  (Link  3  in  Figure  49)  when  they  come  within  1 .6 
cm. 


Arm  A  Arm  B 


Figure  48.  3-DOF  Path  Planning  without  Obstacle  Avoidance:  State 

Trajectories 
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Arm  A  Link  1  &  Arm  B  Link  3  Arm  A  Link  3  &  Arm  B  Link  1 


Figure  49.  3-DOF  Path  Planning  without  Obstacle  Avoidance:  Distance 

between  Links 

One  of  the  advantages  of  using  DIDO  is  the  ease  in  which  the  problem 
can  be  designed.  For  example,  the  buffer  distance  can  be  tailored  for  each 
potential  collision  based  on  the  physical  dimensions  of  each  link.  In  this  case, 
each  link  was  assumed  to  be  the  same  width  and  dm in=  8  cm  for  all  six  potential 
collisions.  DIDO  was  run  using  an  initial  three-point  guess  for  a  16-node  solution 
and  then  a  bootstrap  approach  for  subsequent  iterations.  The  middle  point  of 
this  guess  was  nominally  chosen  so  that  both  arms  would  orient  vertically  with  9i 
at  the  midpoint  the  same  as  0iO.  Intuitively,  raising  the  arms  to  vertical  before  the 
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base  turns  would  create  separation  and  provide  the  arms  with  an  obstacle  free 
path  in  most  cases.  In  general,  this  was  found  to  reduce  the  computation  time. 
Figure  50  sketches  the  60-node  optimal  path  of  both  arms  computed  by  DIDO 
with  an  dmin  =  8  cm.  The  obstacle  avoidance  maneuver  took  the  same  time  as 
the  the  case  with  dmin  =  0  cm,  10'8  seconds,  with  an  increase  in  the  quadratic  cost 
of  3.5%.  Figure  51  -Figure  53  display  the  state  and  control  trajectories,  costate 
values,  and  Hamiltonian  values  computed  by  DIDO. 
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Figure  50.  Cooperative  Path  Planning  for  Dual  3-DOF  Arms  with  8  cm  Buffer 
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Figure  51 .  3-DOF  Cooperative  Path  Planning  using  Geometric  Algorithm  and 

8  cm  Buffer:  State  Trajectories 


Arm  A  Arm  B 


Figure  52.  3-DOF  Cooperative  Path  Planning  using  Geometric  Algorithm  and 

8  cm  Buffer:  Control  T rajectories 
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Figure  53.  3-DOF  Cooperative  Path  Planning  using  Geometric  Algorithm  and 
8  cm  Buffer:  Costate  and  Hamiltonian  Values 


While  the  Hamiltonian  value  is  constant  at  -1  from  Figure  53,  the  value  for 
Xei  for  Arm  A  does  not  appear  to  be  constant  and  steps  up  at  the  midpoint  of  the 
maneuver.  Changing  the  scale  of  the  chart  and  plotting  the  Arm  B  A,0i  values  on 
top  of  the  Arm  A  A,ei  shows  that  this  step  up  is  of  the  same  order  of  magnitude  as 
the  oscillations  in  the  other  costate  values  due  to  numerical  computation  errors 
seen  in  Figure  54.  Figure  55  illustrates  that  the  avoidance  algorithm  does 
provide  the  required  buffer  of  8  cm  between  the  links. 
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Figure  54.  3-DOF  Cooperative  Path  Planning  using  Geometric  Algorithm  and 

8  cm  Buffer:  A.ei  Plot 


Arm  A  Link  1  &  Arm  B  Link  3 


Arm  A  Link  3  &  Arm  B  Link  1 


Figure  55.  3-DOF  Cooperative  Path  Planning  using  Geometric  Algorithm  and 

8  cm  Buffer:  Distance  between  Arms 
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To  ensure  feasibility  of  the  solution,  the  DIDO  computed  control  trajectory 
was  linearly  interpolated  and  used  to  propagate  the  system  using  Matlab’s 
ODE45  function  and  a  fixed  time  step  of  0.05  seconds.  The  results  of  the 
propagated  states,  shown  in  Figure  56,  verify  the  feasibility  of  the  control 
solution.  The  KKT  algorithm  was  also  run  using  the  endpoint  constraints  from 
Table  7  with  nearly  identical  results  to  the  geometric  algorithm.  Figure  57 
compares  the  two  state  trajectories  and  Figure  58  plots  the  costate  values  that 
correspond  well  with  the  geometric  algorithm  shown  in  Figure  53. 

Arm  A  Arm  B 


Figure  56.  3-DOF  Cooperative  Path  Planning  using  Geometric  Algorithm  and 
8  cm  Buffer:  Propagated  State  Values  (o)  compared  with  DIDO  values  (-) 
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Figure  57.  3-DOF  Cooperative  Path  Planning:  State  Trajectory  (KKT  Algorithm 

versus  Geometric  Algorithm) 
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Figure  58.  3-DOF  Cooperative  Path  Planning  using  KKT  Algorithm:  Costate 

Values 
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While  the  solution  paths  between  the  KKT  and  geometric  algorithms  are 
similar,  two  differences  are  worth  pointing  out.  Using  the  KKT  algorithm,  DIDO 
computed  a  cost  function  that  was  7%  lower  than  when  it  used  the  geometric 
algorithm.  Since  the  final  maneuver  time  was  the  same  to  10'8  seconds,  the 
difference  corresponds  to  a  21%  decrease  in  the  quadratic  cost.  While  the  KKT 
algorithm  seems  to  converge  to  a  better  cost  function,  a  computational  price  is 
paid.  On  average  DIDO  took  three  times  longer  to  converge  to  a  solution  using 
the  KKT  algorithm  than  using  the  geometric  algorithm.  The  fact  that  the  KKT 
algorithm  converges  to  a  slightly  lower  cost  solution  infers  that  the  geometric 
algorithm  may  not  be  as  accurate  for  complex  systems  and  the  KKT  algorithm 
may  be  more  robust. 

Whether  this  computational  penalty  can  be  alleviated  is  an  interesting  path 
for  future  work.  In  general,  with  the  3-DOF  problems  visited  in  this  work,  the 
costate  values  were  an  order  of  magnitude  smaller  than  the  corresponding  state 
values.  This  brief  comparison  may  indicate  that  the  problem  can  be  better  scaled 
and  balanced  using  some  unknown  designer  units,  which  would  allow  the 
pseudospectral  method  employed  by  DIDO  to  converge  faster  to  a  more 
accurate  optimal  solution. 
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V.  CONCLUSION  AND  RECOMMENDATIONS 


Application  of  pseudospectral  methods  for  motion  planning  of  multiple 
mulit-DOF  robotic  manipulators  was  studied.  The  use  of  DIDO  for  obtaining 
optimal  trajectories  allowed  the  focus  of  effort  to  be  placed  on  the  problem 
formulation  instead  of  solving  the  optimal  control  problem.  Rather  than  starting 
with  a  complex  problem  that  incorporates  trajectory  planning  and  obstacle 
avoidance,  a  staged  approach  was  found  to  be  an  effective  means  of  developing 
the  final  optimal  control  problem  formulation.  First,  a  relatively  simple  2-DOF 
problem  was  formulated  and  solved  with  all  the  necessary  conditions  for 
optimality  derived  and  verified.  Building  on  this  basic  problem  formulation,  the 
complexity  of  the  system  was  increased  to  3-DOF  and  then  with  the  addition  of  a 
second  robotic  manipulator  a  6-DOF  system.  Pseudospectral  techniques  were 
effective  in  quickly  solving  optimal  pick-and-place  paths.  While  not  trivial,  the 
addition  of  higher  DOF  arms  and  increasing  the  number  of  arms  can  be 
accomplished  relatively  simply  within  the  pseudospectral  framework. 

The  choice  of  cost  function  is  an  important  element  that  has  a  major  effect 
on  the  ultimate  solution.  While  a  minimum  time  trajectory  is  desired,  some  level 
of  efficiency  should  be  included,  particularly  when  dealing  with  decoupled 
elements,  or  even  lightly  coupled  elements  as  is  often  the  case  with  multiple  links 
and  arms.  A  weighted  cost  function  that  includes  both  a  minimum  time  element 
and  a  measure  of  minimizing  effort  is  desirable.  Once  again,  the  use  of  DIDO 
allows  the  problem  formulation  to  be  flexible  and  easily  accommodate  a  wide 
range  of  cost  functions  based  on  the  mission  objectives. 

Even  the  relatively  simple  model  of  a  robotic  arm  using  rigid  links  and 
perfect  joints  had  a  fairly  complex  form.  A  higher  fidelity  model  that  includes  joint 
parameters  and  system  flexibility  can  be  substituted  in  for  the  dynamics  of  the 
system.  While  the  level  of  effort  to  model  the  system  would  surely  increase,  the 
optimal  control  framework  may  exploit  the  more  complex  interactions  to  find  a 

solution  that  increases  the  performance  of  the  system. 
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Obstacle  avoidance  was  included  by  defining  path  constraints  that 
consisted  of  the  minimum  distance  between  the  manipulators’  links.  Using 
parametric  equations  to  define  each  link,  an  optimization  problem  was  formulated 
and  simultaneously  solved  to  determine  the  minimum  distance  between  each  link 
and  any  potential  obstacle.  Two  techniques  are  presented  that  take  advantage 
of  the  flexibility  of  problem  formulation  in  DIDO.  The  results  suggest  that  for 
higher  order  systems,  the  geometric  function  for  finding  the  minimum  distance 
converges  faster  when  provided  a  feasible  guess.  The  algorithm  based  on 
numerically  solving  the  KKT  conditions  seems  to  be  better  behaved  and  solves  a 
low-node  problem  with  a  simple  two-point  guess  more  reliably.  As  the  complexity 
of  the  system  increases,  the  robustness  of  the  KKT  algorithm  can  be  used  to 
solve  a  low  node  solution,  which  can  then  be  used  as  a  guess  for  the  geometric 
algorithm  to  refine  the  trajectory  with  less  computation  time.  The  refined  control 
solution  can  then  be  interpolated  and  used  fora  physical  implementation. 

As  implemented,  the  use  of  the  proposed  obstacle  avoidance  techniques 
requires  knowledge  of  all  obstacles  in  the  workspace.  It  also  assumes  that  each 
link  in  the  system  is  modeled  as  a  line,  which  may  not  be  ideal  for  some  systems. 
Further  efforts  can  be  placed  into  modifying  the  parametric  distance  function  for 
complex  shapes. 

By  not  having  to  focus  on  an  analytical  solution  to  the  optimal  control 
problem,  variations  in  problem  formulation  can  be  explored.  Being  able  to 
compare  the  results  of  different  problem  formulations  may  result  in  the  discovery 
of  computational  efficiencies.  Here,  no  scaling  was  done  on  any  variables.  By 
experimenting  with  designer  units,  the  computation  time  to  solve  the  problem  can 
be  improved  with  no  change  in  the  physical  trajectory.  Another  variation  not 
considered  in  this  study  is  the  use  of  the  links’  minimum  distance  function  in  the 
cost  as  a  pseudo-repulsive  force  or  penalty  function. 

While  the  robotic  manipulators  studied  here  are  relatively  simple,  the 
techniques  presented  can  be  used  to  solve  more  complicated  problems  that  just 
a  few  years  ago  were  considered  unsuitable  for  real-time  use  [5],  Collision 
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avoidance  and  trajectory  planning  for  cooperating  arms  using  pseudospectral 
methods  can  also  be  studied  for  real-time  autonomous  implementation  provided 
computational  efficiencies  are  exploited. 
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APPENDIX  -  2-DOF  ARM  DYNAMICS  COMPUTATION 


Step  1 :  Construct  the  DH  transformation  matirx  and  its  inverse... 


>  H  ~  Matrix(  [  [cos(0).-cos(a)  sin(0).  sin(a)  sin(0),  tf-cos(o)  ],  [sin(0).  cos(a) 
-cos(0).-sin(a)  cos(0),  <7 •  sin ( 0 )  ].  [0.  sin(a),  cos(a),  d  ].  [0.  0.  0.  1  ]]); 

Hinv  :=  Matrix ( [  [cos(0).  sin( 0).  0. [  -cos(a)  *  sin(0).  cos(a)  *  cos( 0).  sin(a).-</ 


*  sin(a)  ],  [siu(a)  *  sin(0),-sin(a)  *  cos(0).  cos(a).-</*  cos(a) 
cos(0)  -cos(a)  sin(0)  sin(a)  sin(0)  rt’cos(o) 
sin(0)  cos(a)  cos(0)  -sin(a)  cos(0)  tfsin(0) 


H\= 


sin  (a) 
0 


cos(a) 

0 


d 

1 


[0. 0.  o.i]]); 


Hinv:= 


cos(0)  sin(0)  0  -a 

-cos(a)  sin(0)  cos(a)  cos(0)  sin(a)  -</sin(a) 
sin(a)  sin(0)  -sin(a)  cos(0)  cos(a)  -^/cos(a) 
0  0  0  1 

Step  2:  Construct  the  link  frame  transformation  matrices... 


Rule  for  D-H  transformation: 

1.  translate  along  the  z_(i-l)  axis  a  distance  d_i 

2.  rotate  about  z_(i-l)  axis  an  angle  theta_i  to  bring  x_i  axis  into  alignment  with  x_(i-l)  axis 

3.  translate  along  x_i  axis  a  distance  a_i 

4.  rotate  about  x_i  axis  an  angle  alpha  i 


D-H  Table: 


link  alpha  a  d  theta 


1 

2 


+Pi/2  al  dl  theta  1 
0  a2  d2  theta  2 


>  //  ~  2  : 

>  HOI  :=  map^x— >ei’a/^ =4-  a  =  d  =  dr  0  =  0^  .rj  j,#  |’ 
map ^  .1 — >era/^  si/bs (o.  =  0,  a  —  a^.  d  =  d%,  0  =  0V  .r ^  V  // ] ; 


*  Courtesy  of  Dr.  Mark  Karpenko,  Guidance  and  Control  Laboratory, 
Naval  Post  Graduate  School,  Monterey,  CA. 
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cos^Gj) 

0 

sin(0i) 

"lcos(0l) 

HOI  \— 

sin(Gi) 

0 

-cos^Gj) 

"l  sin(9i) 

0 

1 

0 

4 

0 

0 

0 

1 

cos(Gt) 

- 

sin(02)  0 

a2  cos^G^ 

HI 2  := 

sin^G-,  j 

cos(G2)  0 

a2  sin^G^ 

0 

0  1 

0 

0  0 

1 

Step  3 :  forward  transformation  matrices 


>  H02  ~  Multiply  ( HOI,  HI 2 ) ; 

H02\=  [  [cos^Gj )  cos(0,),  -cos^Gj)  sin^G, ).  sin^G^.  cos^G^  a 2  cos( 

+  <7j  COS^Gj  j  J, 

|  sin ( Gj )  cos^G,).  -sin^Gj  sin^G,).  -cos^G^.  sin^G^  <7-,cos(0^ 
+  al  sin  ( G  j )  ]» 

|^sin^0^,  cos^G,).  0,  a1  sin^G^  4-  <7^. 

[0.  0.  0.  l]] 


02)  +  sinfGj)  d2 
—  cos^Gj  j  d2 


>  q2x  =  H02x  4; 
q2y  ■■=  H022  4; 
q2z:=H02XA, 


q2x~  cos^G^  a2  cos^G,  j  +  sin^Gj  j  +  a j  cos^G^ 
q2v  :=  sin (0^  <7,,  cos(  0, )  —  cos(  0^  d2  4-  ^  sin (  G: ) 
<7-?rr  :=  ^  sin  ( 0^  +  dy 


>  CodeGeneration  [  'Mat lab'  ]  ( q2x )  : 

eg  =  cos ( theta ( 1 ) )  *  a(2)  *  cos ( theta( 2 ) )  +  sin(theta( 1) )  *  d 
_( 2  )  +  a  ( 1 )  *  cos  (theta  ( 1 )  )  ; 

>  CodeGeneration  [  'Matlab'  ]  ( q2y ) 

cgO  =  sin( theta ( 1 ) )  *  a(2)  *  cos ( theta ( 2 ) )  -  cos ( theta ( 1 ) )  *  d 
_( 2  )  +  a(l)  *  sin( theta ( 1 )  ) ; 

>  CodeGeneration  [  'Matlab'  ]  ( q2: ) 

_cgl  =  a(2)  *  sin ( theta ( 2 ) )  +  d(l); 


Step  4:  Link  Jacobians 
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[> 

Rotation  matrices 


>  R00 
ROl 
R02 


=  IdentityMatrix  ( 3 ) ; 

=  SubMatrix ( HO  1,  [1  ..3],  [1  ..3]); 
=  SubMatrix ( HO 2,  [1  ..3],  [1  ..3]); 


R00- 


1  0  0 
0  1  0 
0  0  1 


R02  := 


Position  matrices 

>  rOl  —  SubMatrix ( HOI.  [  1  .3 ],  [4]); 
rl 2  :=  SubMatrix ( HI 2,  [1  ..3],  [4]); 


rOl  := 


cos^Gj 

)  0  sin^Gj) 

ROl 

:= 

sin^Gj 

o  - 

cos^G^ 

0 

1 

0 

COS^01  j 

cos 

•ft)  - 

cos(  Gj 

)  sin(e2) 

sin^G^ 

sin  (9^ 

cos 

ft)  - 

sin^Gj 

)  siu(e2) 

-cos^G^ 

sin  | 

ft 

cos 

ft) 

0 

al  C0S(Gi) 

/7j  sin/Qj  j 


r!2~ 


a~  cos 


a2  sin 


ft) 

in(e2) 


d. 


Z-vectors 

>  =base~  vector{[0.0.  1]): 

=0  —  Multiply  (  R00,  =base\\ 
zl  :=  Multiply (ROR  z^V 


base ' 


0 

0 

1 
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. 0 := 


zl\= 


_mk  mass-centers 

K2<-  Matrix^  [  [A,],  [&,]]); 


sm 

-cos 

0 


(®>) 

W 


Pll := 


AV:= 


*i 

/Z. 


Transformed  mass-centers 

>  Pllsrar  ==  Multiply  { ROl,  Pll ) ; 

POlsrar  ■=  Multiply  { R00.  rOl)  +  Pllstar. 

'  008(0,)  /r1  +  sm(01)^1 
Pllstar  :=  sm  ( 0. )  Zt^  -  cos(  6. ) 

cos(  0. )  -I-  cos(  0. )  +  sin(  0. )  t j 

POlsrar  :=  a^  sin(0j )  +  sm(01 )  Zij  -  cos(  0. )  lzx 

4  + 


>  Pllsrar  ■=  Multiply  {ROl  P22)\ 

Pllstar  •■=  combiue{Multiply{R01,  rll) )  +  Pllstar 
POlsrar  ■=  co/nbine( Multiply (R00.  rOl) )  +  Pllstar : 
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P22star  = 


cos(0.  j  cos(0,)  lx,  -  cos( Gj )  sin(0,)  h\  +  sm(0])  /r, 
siq^  0j  j  cos(0, )  Zr,  -  sm(0. )  sini'©,)  /);  -  cos^Oj  lz2 
sin(0,)  lx,  +  cos(0,) 


P12star  := 


-y  a,  cos(0.  —  02 j  +  y  a,  cos(0j  +  0,j  4-  sm(0j)  d,  +  cos^)  cos(0.,  )  lx. 


-  cos(0j)  sin(0,)  /} j  +  sm(0j)  t2 
-y  a,  sm(0j  +  0;)  +  y  a ,  sm(0j  —  0,)  —  cos(0j)  d,  +  sin(0j  )  cos(0.,)  lx, 

-  sm(0. )  sin(0,)  ly2  -  cos^)  Zr, 
a,  sin(0,)  +  sin(  0,)  lx,  +  cos|0,  |  fy. 


Build  the  Link  Jacobians 

>  zeros  :=  Transpose ( Vector^  1 0,  0,  0.  0,  0,  0 1 ) ) : 

Jill  '■=  Transpose.)  Vector ( [  zOSci  Fector(  [  POlstar)),  zO])) : 
JI21  ■=  zeros : 

JI1  ■=  combine)  ( Transpose)  Matrix)  \  \  Jill),  \  JI21  ]])))); 

-ax  sm(  0t  |  —  smf0I )  Ix^  +  cos(  0. )  /y  0 


jir. = 


cos( 0, )  +  cos( 0]  j  ZTj  +  sin(  0j )  lzx  0 

0  0 

0  0 

0  0 

1  0 

>  J112  •■=  Transpose ( Vector ( [zOStx  Vector ( [  P02star ] ),  zO] ) ) : 

JI22  ••=  Transpose ( Vector ( [zl&x  Vector([P12star] ),  zl])): 

JI2  ■=  combine ( Transpose(  Matrix ( [  [  JI12),  [  JI22 ]  ] ) ) ); 

*i  ^(0j)  -  ^  a2  sin(e]  +  02)  “  -r  a2 sin(0i  -  e2)  +  cos(ei)  d2 

j  Zt2sin(01  +  9;)  -  v  Zc,sin(01  -  0,)  +  j  ^cos^  -  0,)  -  y  ^2cos(0, 

+  0,)  +  cos(0,)  lzy  -  y  a,  sin(0j  +  0,)  +  y  a2  sm(0i  “  02)  ~  T  ^  sin(0i  +  02) 
+  \  lx,  sm(01  -  02)  -  4  (y2  cos(9i  -  e2)  -  T  t>2  cos(0i  +  e2)  I 


J12\= 
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cos(0j)  +  y  <Zj  cos(0j  —  6,)  +  Y  <7,  COs(0]  +  0,)  +  Sl^Gj)  <7  +  y  Zt,  COs(Gj 
02)  +  y  Z^cos(0j  +  0,)  -  y  #2  sm(0j  +  02)  +  y  Z^sin(0,  -  0,) 

+  sm(0j)  Z"2,  -  —  a2  cos(0,  -  0,)  +  —  a2  cos(0j  +  02)  -  —  tc,  cos(0j  -  0,) 

+  y  be,  cos(0j  +  0,)  -  y  ty-i  sm(ei  +  02)  ~  y  sin(0i  “  e2)  ]> 

0,  a2  cos(0,)  +  cos(02)  -  sin(0,)  fy2  , 

[°,  sin(0j)  ], 

[°-  cos(0i)]’ 

.ink  Inertia  Matrices 

>  Ill  ■■=  Matrix^  [  |  Ixxl  Ini  &zl\  |  fyxl,  fyyl  fyzl\,  [  Izxl  Izyl  Izzl  ||) : 

11  :=  Multipty{R01,  Mnlriph\Ill,  Transpose  ROl ) ) ); 

//:=  |  [cos^Gj)  (Ax/cos^,)  + /ur/sin^) )  +  sin(0.)  (Tr/cos^)  +  7r/sin(  Gj), 

cos(0t)  (Ar/sm(0j)  —  /tr/cos^G, ) )  +  sin(0j)  (Tx/sm^  j  -  Izzl cos( Gj ) ) , 
cos(  0I )  Ayl  +  sin(  G. )  Izyl J, 

sin^Gj)  (Av./cos(0j)  +/crAm(G1))  —  cos(Gj)  (Tx/cos^Gj)  +  7z/sm(0,) 
sin^Gj  (Ar/sin^Gj)  — /tr/cos^G^  )  —  005(0^  (Tx/su^Gj  —  Izzl  cos(  0.  }  j , 
sin  ( Gj )  kyl  -  cos(  Gj )  kylj, 

hxl cos(Gj)  +  hzl sm  ( 0 } , Iyxl sin  ( 0 . )  —  AzJ cos( Gj ) , fyyl 1 1 

>  122  ■=  Marrix{  [  |  £oc2,  Ixy2 1  Ivz2\,  [  fyx2,  A. r2  fyz2],  [  Izx2,  Izyl,  Izz2 )  ] ) : 

12  •=  Multipfyy  R02,  Multiply^  122,  fra/ispose(R02) ) ); 

12 :=  |  | cos^Gj  j  co$(0,)  ( Zu7cos( G. )  cos(G.,)  —  ZnZcos^  ]  sin ( 0, )  +  ZizZsm^ ) ) 

-  cos(  Gj )  sm(  G, )  ( /jr7eos(  0t )  cos(  0, )  —  Av7cos(  0J  sin(  0, )  +  ZrZsm(  Gj ) ) 

+  sin(0.)  (TxZcos^Gj)  cos(G,)  —  TyZcos^Gj)  sin(0.,)  +  7rz7sin(0. 

cos(  Gj  j  cos(0,)  (Ar7sin(0j)  cos  (9,)  —  ZryZsm^Gj)  sin  ( 0,  j  —  Ar7cos(0j  j ) 

-  cos(G] )  sin  (6,)  (/ir7sm(9,  j  cos(G,  j  —  7i7sm(0. )  sm(G. )  —  7z_',cos(61 ) ) 

+  sm(G,  J  (7a7sin(0])  cos(0,j  —  7175^(0^  sin(0,)  —727005(0^), 
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cos(Gj)  cos(0,)  ( Ar7sin(0,)  +  Zj'7cos(0,) )  -cos^Gj)  sin ( 0n )  (Ar2sin(0,) 
+  /m7cos(G,) )  +  sin^Gj)  (Zx7sin(e,)  +  Ay.7cos(0,))  |. 
sin|0j)  cos(0,)  (Ar7cos(0j)  cos^G.,)  —  ZyPcos^Gj)  sin(0.,)  +  ArAin^Gj)) 

-  sm(G.  )  sm^G,)  (  7k£<?cqs(  0[ )  cos^G^  —  At7cos(G1)  sin(0, )  +  Tjzi’sin^Gj )  l 
-cos(Gj)  (ArJ’cos(01)  cos(0.,)  —  Zj:7cos(  Gj )  sin(0,)  +  ZzZsin^Gj)), 

sin  (9. )  cos(0^  (Zx?sin(0j)  cos^G,  j  —  Aj^sinf  G^  sin(0^  -  Ar2cos|GI  j ) 

-  sin( Gj )  sin( 0,  j  ( /ir7sin( 0i )  cos( ®-> )  —  An7sin( G.  )  sin( G-,  )  —  M2 cos( Gj )  j 
-cos^Gjj  (ArJ’sin(01)  cos(0,)  —  A>,i’sin(01  j  sin(0^  —  Zz7cos(0.  )  ), 
sinfGj)  eos(0.,)  (Ar7sin(0,)  +  AjAosfe, )  )  —  sin  ( Gj  j  sin(0,)  (Ar7sin(e.i 
+  ^4?cos^01JJ  —  cos|0jj  (Zx7sin(6.)  +  Zy7cos(G,) )  ], 

sin ( 0, )  (  Ai'7cos(  Gj )  cos(  0, )  -  AyZcos^  0t  J  sin  ( 0, )  +  ArZsin ( Gj ) ) 

+  cos(G,j  (7j:r..7cos(0,  )  cos(0.)  — ZtZcos^Gj  ’i  sin(  9,)  +  ArJsinj^  j  j, 
sin^oA  (/n‘7sin(G1)  cos( 0,  j  -  AjAsinf  Gj  j  sin(0,)  —  Ar7cos(  0t ) ) 

+  cos(G,  j  (7ix..7sin|'9,j  cos^G,  j  -  Ar7sm(9. j  sm(0,j|  -  ArJeos^  )  J, 
sin(0,)  (Ari’sin(G,)  +  /rv7cos(9.,) )  +  cos(0,)  ( Av_7sin(0, )  +  Ar7cos(0.,) )  ] 


exploit  symmetry  in  inenia  matrix. 

>  Ml  ■=  Ml  :  Ml  :=  Ml:  Ml  '•=  Ill'- 

>  M2  :=  M2 :  lzx2  ■=  M2:  M2  ■=  M2: 122: 

vlampulator  Inertia  Matrix 
Translational  Energy 

>  Mil  :=  Multiply  ( Transpose{SubMatrix{  Ml  [1  ..3],  [1 ..//]) ),  mx  •  SubMatri\{JlL  [1  .3],  [1 

"])); 

>  Mt2  ■■=  Multiply (  Transpose{  SubMatrix{  J12 \  [  1  ..3  J,  [1.7/])),  ///,  •  SubMatrix{J12 \  [1.3],  1 1 

*])); 


Mt2  := 


\  f  ~ox  sm(0j  j  -  —  a2  ^in(Gj  +  0,)  -  y  u2sin(91  -  0;)  +cos(Gj)  d2 

y  sin(e1  +  e2)  -  y  &,  sm(ej  “  e>)  +  T  ^2  cos(9i  “  e2)  “  y  cos(0i 

+  0,)  +  cos(Gj )  lz2  |  +  /«,  |  ax  cos(0] )  +  cos^Gj  —  0,)  +  y  cos^Gj 

+  0,)  +  sm(G1)  d2  +  02)  +  y  lx2co‘>(9l  +  92)  -  y  ^  sin(0j 


( 
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+  0:)  +  sin(ei  —  e2)  +  sm(ei)  &2)  *  ^  -^1  sm(Qi)  -  l-^sm(ei  +  0;) 

y  a2  sin(01  -  02)  +  cos(0,)  <  -  y  Zc,  sin(01  +  0,)  -  y  lx,  sm(0j  -  0;) 

+  y  /p2  cos(0t  -  0,)  -  y  fy2  cos(0,  +  0,)  +  cos(0,)  Zr2  j  f-y  a,  sin^  +  0,) 
+  y  -  02)  -  y  ^sin(01  +  02)  +  y  Zc,sin(0j  -  0;)  -  y  ^cos^ 

-  e2)  “  y  fy2  cos( 0j  +  02)  ]  +  I  ax  cas^)  +  y  a,  cos(0,  -  02)  +  y  a2  cos(0j 
+  02)  +  Sm(0j)  d2  +  y  ^cos(0j-02)  +  y  Zc,cos(0j  +  02)  -  y^sin^ 

+  e2)  +  y  fy2  -  02)  +  sm(0j)  lz^m,(-ja2  cos(0,  -  02)  +  y  a ,  cos(0j 

+  e2)  _  y  ^  cos(ei -  e2)  +  y  /x2  cos(ei +  e2)  -  y  ^2 sin(0i +  e2) 

-  y  ^2sm(0j-02)  j  . 

[^sm^)  -  y  <7,5111(^  +  0,)  -  i/72sm(01  -02)  +cos(0j)  d, 

y  sm(0!  +  02)  -  y  lx,  sm(0j  -  0,)  +  y  fy2  co$(0j  -  02)  -  y  fy2  cos(0j 
+  0,)  +cos(0j)  lz,  j  m2  j^-y  ^2sin(0j +  0,)  +  y  a,  sin(0,  -  0,)  -  y  Zi2sin(01 

+  02)  +  y  ^  sin(0i  -  e2)  ~  y  *2  COS(01  ~  02)  ”  y  cos(ei  +  e2) ) 


(  ,  K  ,  1  /  V 

1  /  V 

<7,  cos(0jj  +  —  a2  cos(0j  -  0,) 

+  —  <Z,COS(01  +  0,j 

+  sm(0j) 

+  4*  /xi cos(ei  “  02) +  y  Zr2 cos(ei +  02)  -  t  & sin(ei +  e2) +  y  h sm(ei 

-  0,)  +  sm(0j)  lz2  j  /«J  y  <Zj  cos(0j  -  0,)  +  y  a2  cos(01  +  02)  -  y  lx,  cos(0j 

02) +  y  ^ cos(ei +  02)  ~  y  sm(ei +  e2)  ~  y  sin(0i -  e2)  J-  mz  ( 

-  T  a2  Sin(0l  +  Qz)  +  -T  a2  Sin(0l  e2)  “  T  ^2  s“(0l  +  02)  +  y  /x2  Sin(0l 

e2)  -  y  fy2  cos( 0j  -  02)  -  4-  ty-i  COS(01  +  02)  I  +  m,  f  -  y  a2  cos^  -  0,) 
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+  T  *2  C0S(61  +  e2)  -  y  Lx2  cos(ei  -  e:)  +  T  ^2  COS(01  +  0:)  ”  4"  sm(ei 

1  2I 

+  02)  -  j  ^sinje,  -  02)  J  +  m2  (*,cos(02)  +  eos(02)  A,  -  sin(02) 

_> 

Rotational  Energy 

>  Mrl  :=  Multiply ( Trampose{ SubMatrix{  Ml,  [4.6),  1 1  ..n  \ ) ),  Multiph\Il,  SubMarrix{Ml. 

[4-6],  [1. '//]))); 

lyyl  0 

Mrl- 

0  0 

>  Mr 2  ••=  Multiply ( Transpose{  SubMatrix{  M2.  [4.6],  [  1  ..n  ] ) ),  Multiph\IZ  SubMarrix{M2. 

[4-6],  [I  .77]))); 

Mr2.=  |  |  sin ( 0,  'j  (Ar^sin(0,)  +  /rji’cos^©, )  )  +cos(0,)  ^A:r_,sm(0^ 

+  {yy2aos{Q^\},  (sin(0,)  (Ari’cos^Oj  cos( 0,  j  —  Ak?cos(  0.  )  sin(0,) 

+  Az?sin^0j)J  +  cos( 0,  j  (/rxAos^ )  cos(0,)  —  AtJ,cos(0] )  sin (0,1 
+  ^2i’sin(01) ) )  sin^O,)  —  (sm(9,)  (Ar^sin^)  cos(0,)  —  AH?sin(0j)  sin(0.,) 

-  j  +cos(0,'|  (Ax_7sm(0. )  cos(0,)  —  ArJ’sin^©. ')  sm(0, ) 

-  fyz2cos(  9j ) ) )  cos(  0j )  j, 

sin^0jj  |cos(0jj  cos(0,)  (Ztx?sin(02)  +  Avi’cos|0;^ 

-  cos(0j)  sin(0,)  ( 7jT_7sin(0^  +  Ari’cos^O,) )  +  sin(0, )  (ArJsin(0,) 

+  .£^005(0,)))  —  eos^0j)  (sin^)  cos^0,j  (Ai:?sm(0,)  +  A>s?cos(  0.,  j  j 

-  sin(0j)  sin(0„)  (Z)x?sm(02)  +Zj^cos(0,))  -  cos(0.)  (Ar,?sin(0,) 

+  Jzy2cos(  0, ) )  j ,  sin  ( 0t  )  ( ( cos(  0.  j  cos(  0, )  ( Ar.?cos(  0j )  cos(  0, ) 

-  /ni’cos(01)  sin(0,)  +A"?sin(91)l  —  cos(  0t  ]  sin  ( 0,  j  (Ax^cos^O, )  cos(  0, ) 

—  Iyy2 cos^0j)  sin(0,j  +Z^?sin^91)  j  +  sin^O^  (Ax?cos(01)  cos(0,) 

-  Aji?cos^0j)  sin(0,)  +Az7sm(9j)))  sin(0j) 

(cos^Oj)  cos(0,)  (Ar-Psin^j  cos(0,)  —  Ayi’sm^Oj  sin(0,)  —  AzAos^Oj  j 

-  eos(0j)  sin(0,)  (/jxZsin^O^  cos( 0, )  —  Arising 0. )  sm(0,)  —  Asi’cos^ 0j ) ) 

+  sin(0.)  (Ax?sin(0j)  cos|0^  —  Zx'J’sin^ )  sin(0,)  —  lzz2  cos^Q^  'j  j  cos|'0.)  ) 

-  cos(0j)  ((sin(0j)  cos(0,)  (Ar*?c os(0j)  cos(0,)  —  Ar^cos^Oj)  sin(0,,) 

+  Az^sin^QjJ  J  -  sm(0. )  sin(0, )  (^xAos^ )  cos(0,)  -  AyJ cos(Q.  j  sin(  0, ) 
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+  7>z7sm(0])  )  —  cos(9])  (ZxZcos^Gj]  cos(0,)  —  ZtZcos^G.)  sin(0,) 

+  Izz2sin( 9j )  j )  sin^Gj  -  (sin^Gj  cos(0,)  (TcrZsin^Gj  cos(0, ) 

-  /rj'^sin^©])  sin(0„)  -  /tr_7cos( ex  j  j  —  sin(0j)  sin(0.,)  (.^xZsin^Gj )  cos(0, ) 

-  ^?sm(0j)  sin(0,)  -  .#rZcos(  0X ) )  —  cos(0j)  (.£v.i,sin(01  j  cos(0,) 

-  Zrj^sin^Gj )  sm(  02 )  -  Izz2 cos(  0t ) ) )  cos(  0j ) )  ] 


> 

Total  Energy 

[>  Af  •■=  combine^  Mtl  +  Mt2  +  Mrl  +  Mr 2) : 
Veclocity  Couplmg  Vector 
>  V~-=  Vector (//,  0} : 

for  /from  1  to //do 
for/ from  1  to //do 
for  ./from  1  to //do 


V 


V.+  eval 
/ 


U 


end  do: 
end  do: 


d  1  d 

- M  -i- - M 

d0.  Ay  2  d0  Jk 

*  t 


> 

■10  -CO. 
J  * 


end  do: 

combine ( V)\ 

2  m,  d,Iz,—  2  m,  ax  h\  sin(  0, )  +  —  lxx2  +  2  m,  ax  a,  cos( 0,  j 

+  2  rn,  ax  be,  cos(0,)  —  rn,  lx,  ly,  $in(2  0,)  +  m,a,lx,  — rn,  fit  cos(2  0,)  +  -y-  m, 
hq  +  y  m,  a;  +  —  m2  ly\  +  y  Ax?sin(2  0,)  -  m,  a2  ly2  sin (2  0,) 

+  rn,  a2  lx,  cos(2  0.,)  +  —  7n7  co$(  2  0,)  — ^  7cr7cos(2  0,)  +  lyyl  +  y  Ay 2  +  rn, 

crx  +  rn,  lz$  +  ///,  d;  +  y  ///,  cq  cos(2  0,)  +  ~  rn,  lx;  cos(2  0,)  +  y  Acy2  sm{2  0,), 

-m,  d2  fy2  cos( 0,  j  —  m,  t2  lx,  sin(0,)  —  rn,  Iz ,  a,  sm(0,)  —  rn,  Iz ,  fy2  cos^G,) 

-  rn,  d2  a,  sm(  0, )  -  rn,  d2  lx,  sm(  0, )  4-  7iz7co$(  0, )  +  Acz2$m(Q,  j  |. 


—  Mtl  + 

00,  l 


ZzvZsin^  0, )  +  Z7’7cos(  0,)  -  m,  d,  h;  co$(  0,)  -  m,  Iz,  lx,  sin(  0, ) 
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-  m2 12  a2  sin(0,)  -  m,  Iz,  fy2  cos(0,)  -  m,  d,  a2  sin(0,)  -  d2  lx,  sin(0,), 

2  m,  a,lx,  +  m,ly  +  rn,  a;  +  m,  ly  +  jfe?]  ])  )  a>j  ca,  +  -7^-  |  MtJ 

r  1  1,  1 

+  2  m,  d,  t,  -  2  m2  ax  fy,  sin(  0, )  +  —  Loc2  +  2  m,  ax  a2  cos(  0, ) 

+  2  m2  ax  lx2  cos(0,)  -  rn,  lx,  ly2  sm(2  0,)  +  m2  a2  lx,  -  y  ///,  ly  cos(2  0,)  +  y  /», 

+  y  JW,  +  y  rn,  ly,  +  y  Arising 2  0,)-  m2  a2  fy2  sin(  2  0, ) 

+  rn,  a,  lx,  cos(  2  0,)  +  y  Ay2co$(  20,)  — 7-  Ixx2 cos(  20,)+  fyyl  +  y  M2  +  rn, 

ax  +  rn,  Iz;  +  /«,  </T  +  y  /as,  ^  cos(2  0, )  +  y  rn,  Zt?  cos(2  0,)  +  y  /o'-7 sin (2  0,), 

-m,  d,  fy2  cos(0,)  —  m,  t2  lx,  sin ( 0, )  —  rn,  Iz ,  a,  sin ( 0, )  —  rn,  lz2  fy2  cos(0,) 

-  rn, d2  a,  sin(0,)  -  m,d,  lx,  sin(0,)  +  I\z2c<x>[ 0, )  +  Zj-?sin(0,)  1, 

[-£xi’sin(0,)  +  ^4?cos(02)  -  m,  d,  fy2  cos(0,)  -  nr,  Iz,  lx,  sin(0,) 

-  m2 12  a,  sin(0,)  -  nr,  t,  fy,  cos(0,)  -  m,  d,  a,  sin(0,)  -  rn,  d,  Lx,  sin(0,), 

2  m,  a,  lx,  +  m,  bC,  +  rn,  a1,  +  rn,  ly  +  Izz2\  | )  )  co*  j. 


y(i( 


MJ  + 


2  nr,  d,  Iz,  -  2  rn,  ax  /j4-Sin(0, )  +  y  Ixx2 

+  2  nr,  a}  a,  cos(  0, )  +  2  rn,  ax  Lx,  cos(  0, )  -  rn,  lx,  fy,  sm(2  0,  )  +  rn,  a,  lx,  -  y  nr, 
h\  cos(2  0,)  +  y  r»2  '4  +  T  m2  a\  +  y  +  j  ^sin(2  0,) 

///,  tf„£;sin(20,)  +  nr,  a,  lx,  cos(  2  0, )  +  y  fyr2cos(  2  0, )  -  yAu?cos(20,) 
+  M'l  +  y  M2+  rn,  of  +  nr,  lr,  +  nr,  d;  +  y  m,a2  cos(  2  0,)  +  y  m, 

bq  cos (2  0,)  +  y  Ah?  sin  (2  0,),  -nr,  d,  ly2  cos(0,)  -  m,  Iz,  lx,  sin(0,) 

-  rn,  lz2  a,  sin(0,)  -  nr,  lz2  fy,  cos(0,)  -  rn,  d,  a,  sin(0,)  -  rn,  d,  lx,  sin(0,) 

+  /ja?cos( 02 )  +  /t+'sin^O,)  j. 
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Av2sin(0, )  +  A»'2cos(0,)  -  w,  d^  Zc,  cos(0,)  -  m2  Zr,  Zv,  sm(0:) 


-  ^Z2^2sin(e2) 


w,Z:,  A;cos(0,) 


-  ///,  d2  <7,  sm(0,j  —  //7,  Z,  Zv,  sin  ( 0, ) , 


2  ///,  <7,  Zt,  +  m2  Zt;  +  /«,  a;  +  ///,  A;  +  Jzz2  J  ] )  )  coj  +  |  W  + 

1  i,  i 

2  /ttj  d2  lz2  —  2  w,  ^  A;  sin  ( 0,  j  +  —  lxx2  +  2  ^  a2  cos(  0, )  +  2  /«,  <7;  Zv,  cos(  0, ) 

-  m2  Zr2  A,  sin{2  0,)  +  m2  rf,  Zv,  -  4-  w,  A;  cos(2  0,)  +  rn2  Zt^  +  4~  m2  al 
+  y  /«j  A;  +  4-  Av-?sin(2  0,)  —  a ,  A;  sin  (2  0,)  +  w,  a,  Zt;  cos(2  0,) 

+  4-  At7cos(2  02  j  — 4  Av2cos(2  0,)  4-  AyJ  +  4-  Ay2  +  m2  a*  +  w,  lz^  4-  /«, 

+  ^  m2a; cos(2  0,)  4-  4~  m2  ^ cos(2  e2)  +  4"  Ar,?sin(2  0,),  - m \ d2 ly2 cos(0,) 

-  m2 12  Zt;  sin  ( ©, )  -  m}  lz2  a,  sin  (02)  -  m,  lz2  ly2  cos(  0, )  -  w,  <  <7,  sm(0,) 

-  //£,  Z  Zc,  sin(  0, )  +  /jr.?cos(  0, )  +  Arising 0, )  jT 

[Zx?sin(02)  4-  Z^',?cos(0,)  -  m,  7,  A,  cos(0,)  -  w,  Zr,  Zc,  sm(0,) 


-  ///,  Zr,  (7,  sin(  ©,)  -  w,  Zr,  A;  cos(  6, )  -  ///,  Z,  <7,  sin(  0,)  -  m2  d2  Zv,  sin  (6,), 

2  //z,  <7,  Zc,  4-  /«,  be,  +  /«,  <72  +  }tu,  A;  4-  Izz2  ]  j  — j-  -7^—  [  Mtl  4- 

1  2,1 

2  m,  Z,  lz2  —  2  m,  ax  Zr,  sin(0,)  4-  —  Ax? 4-  2  /«,  ^  <7,  cos(0,)  +  2  /«,  <7,  Zt,  cos(0,) 

w.  A,  A;  sin (2  0,)  +  m2  tf,  Zt,  -  4-  A;;  cos(2  0,)  4-  4"  mi +  4"  mz  *2 

4-  4-^+  4-7iaZsin(2  0,)  -  w,  <7,  Zv,  sin(2  0,)  +  w,  <7,  A,  cos(  2  0, ) 
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1  1  1  ,  -  - 

+  —  Ak?cos(2  02)  —  y  Ax?cos( 2  0,)  +  Aj/  +  y  Jf'y2+  m,  d[  +  /«,  ZzJ  +  /«,  ^ 

+  4-  /«,  <7,  cos(2  0,)  +  4"  /w,  Z£  cos(2  0,)  +  4~  1\t2 %m(2  0-,),  -pi,  d2  fy2  cos(0.,) 

-  //it,  lz2  A,  sin ( 0.,  j  -  pl,  Iz2  a,  sm(0,)  —  /«,  lz2  ly2  cos( 0,  j  -  pl,  d2  <7,  sin(0,) 

-  ///,  <7!,  Zc,  sin(  0, )  +  fyz2 cos{  0, )  +  Ar^sin(  0,  j  ]. 

Arising©,)  +  AyZeos^©,)  -  //?,  d2  4;  cos(0,)  -  w,  Zr,  Zt,  sm(e,j 


///2 12  a2  sin(0,)  -  m2 12  4;  cos(0,)  -  /w,  d2  a2  sin(0,)  -  m2  d2  Zr,  sm(0,). 


+ 


’]])  )“■“!-{ 

1  2 

d 

d©7 

Jxx2  +  2  tn,  ax  <7,  cos 

(«0 

Ml 


1  2  1 

+  2  nu,  ox  Zc,  cos(0,)  -  /«,  Zc,  Z^  sin(2  0., )  +  pl,  a,  Zc,  — —  m,  ly  cos(2  0,)  +  —  pl, 
ly  +  4-  »h  d^  +  -7  ,ni  $  +  y  ^^sin(2  0,)  -  /«,  <7,  A,  sin (2  0;) 


4-  ///,  *7,  Zc,  cos|2  0,  j  +  4-  -4r.?cos(2  0,)  — ^  Ax?cos^2  02)  +  At/  +  4"  -&.r'2+  m-> 

y  +  m2  lr2  +  Pt2  d*  +  4-  w,  y  cos(2  0,)  +  y  w,  Z»;cos(2  0,)  +  4-  AyZsin(2  0,), 

- /«,  </,  4^  cos(  0, )  —  /«,  Z:2  Zc,  sin  ( 0, )  —  /«,  /z2  <7,  sin  ( 0, )  —  pl,  lz2  fy2  cos(0,) 

-  ///,  <<7,  sin(0,)  -  pl,  d,  Zr,  sin( 0,)  +  Azi?cos(e^  +  ArZsin(0, )  |. 


(  0, )  +  Jz}2 eos(  0,)  -  pi2  d2  4;  cos(  0, )  -  -w,  A,  Z^  sin  ( 0, ) 
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>rh 


Zy  a  sin 


(Q,)  -  m2 12  fy2  cos(0:)  -  m2  d2  a2  sm(9;)  -  /y  d2  lx2  sin(02). 


2  m2  a2lx1  +  m2bq  +  m2  y  +  /y  ly\  +  jfe?]])  )  to,  (Dj  +  y 


2.  1 


d 

ae. 


MZ 


2  ftxd^t2~  2  /y  y  /i^  sin(0.,)  +  —  Jxx2+  2  /y  y  y  cos(e^ 

+  2  /y  y  Zy  cos(0^  —  /y  Zy  Zi;  sin(2  0^  +  /y  y  Zy  — j-  zy  cos(2  6,)  +  y  zy 


Zt?  +  y  /Bj  +  y  zy  Zj^  +  y  Zkt?sin(2  0,)  -  zy  a2  Zy  sin(2  0,) 

+  zy  y  Zy  cos(2  9:)  +  y  Zj/y?cos(2  0;)  — y  ArJcos(2  0,)  +  fyyl  +  y  Jyy2+  zy 

^  +  zy  l£,  +  zy  d}  +  y  zy  y  eos(2  0,)  +  y  zy  Zt?  cos(2  0,)  +  y  Zry?sin(2  0,), 

zy  y  ^  cos(0,)  —  z«j  Zy  Zy  sm  ( 9,  )  —  zy  Zy  y  sin  ( 0, )  —  zy  Zy  Zy  cos(0,) 

-  zy  d2  y  sin(  0, )  -  zy  </,  A,  sin(  0, )  +  Z^?cos( 0, )  4-  Zir.?sin(  0, )  ], 

Zn-?sin(  0, )  +  Ey2 cos(  6,)  -  zy  d^  A;  cos(  0, )  -  zy  Zy  Zy  sm  ( 0,  j 

-  zy  Zy  y  sin( 0,)  -  m2  Zy  A;  cos(6,  j  -  zy  d2  y  sm(0,)  -  zy  d2  Zy  sin(  9,), 

2/yyZy  + w2A;  +  /y^  +  /y^  + Ar/])  )  to*]] 

Gravity  Vector 

J  2.2 

>  G  ■=  Vector{  n.  0)  : 

gytc-=  l'ector{{ 0,  0,  g<~]) : 

/==  1 : 

for/ from  1  to  //do 

Gj  :=  Gj-m-MuItipfy  ^Transpose  Vector^  [  Sub.\Iatrix{  JI1.  [1..3],  [/])]))  : 

end  do: 

/  :=  2 : 

for/ from  1  to //do 

G  :=  £r  .  /// ;  Multiply  {Transpose  Vector{[SubMatrix{JI2 \  (1  ..3],  [/])]) ) : 

end  do: 

G, 

0 


/y  (y  cos(02)  +  cos(0,)  Zy  -  sin(0,)  Zy) 

Assemble  the  dynamic  equations 
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>  T  ;=  Vector{n.  0) : 

T  :=  Multiply (M  Vector ( |  04.  a,  |) )  +  G+  V: 

T  :=  co/nbine{  T )  : 

T  :=  collect^  T,  {ctj,  a,,  jr} )  : 

> 

>  T  ■=  T 
1  r 

r\  :=  (  dl  a2  Sm(0:)  ”  m2  IZ2  a2  Sin(02)  “  m2  lZ2  ^2  SU1(9:)  “  m2  lz2  &2  C0S(6:) 

—  /«,  */,  cos(0,)  +  Z*r-?sin(0,)  +  AcZcos^O,)  —  in,  d^  Zt,  sm(6, ) )  a,+  ( Iyyl 


,  i  1 

+  2  w,  d?  Zr,  +  Ixy2  sm  ^  2  9,  j  4-  w,  ^  +  w,  Z~  +  y  Iyy 2  +  2  ^  Lxx 

+  4  -AjK,?COS(2  0,)  -  4-  m2  $  C0H2  9i)  +  T  m2  **2  C0S(2  02)  +  m2  °2  lX2  C0S(2  9:) 

-  1  •> 

w,  ly\  sin(2  6, )  +  w,  d;  +  —  w,  lx;  cos(  2  0,  j  +2  m,  ax  ly  cos(  0. ) 


-  w,  A,  A;  sin(  2  0, )  +  Wj  a*  +  mx  A'  +  mx  l2^  —  2  zw,  ^  /);  sin(  0, ) 

y  -Zu.7COS( 2  0,  j  +2  «,  <7j  COS^ 0:)  +  4-  /ixZ  +  Z0,  0,  Zl,  +  y  -W,  <  +  W, 

Zi;  +  4-  A;  |  a2  -  cd'  .AcZsin^O,)  -  to,  /w,  be,  sin(2  0,)  +  a>‘  A.-Zcos(0,) 

+  tOj  a),  w,  A;  sm|  2  0,  ')  +  2  to,  /nZcos^  2  0,  )  —  2  to,  lx,  A;  cos( 2  9,)  + 

2 

to;  m,  d,  A,  sin(  0, )  -  to,  7)y2sm(2  0, )  +  to,  ZixZsm/2  0, )  +- 

to' m,  t2  ly2  sin(0,)  -  to' m,  d2  lx,  cos(0,)  -  tOj  to,  m,  a;  sin(2  0,) 

1 

-2tOjto,  m,  tfjZ^sm^©,)  -  to'  m,  Z:,  A,  cos(  0, )  -  2  tDj  to,  m,  <7,  A;  cos(2  0,) 

*> 

to^/a,*/,  tf,  cos[  0,)  —  2tOjto2w,  a,  Zi;sm(20,)  -  2  tOj  to,  m,  ^  <7,  sin  ( 0, ) 
to2  m,  /_2  <7,  cos(  0, )  -  2  tot  to,  w,  ax  ly2  cos(  0, ) 

>  CodeGeuerar/on\  'Marlad )  ( ZJ ) ; 

cg2  =  (-m(2)  *  d(2)  *  a(2)  *  sin( theta( 2 ) )  -  m(2)  *  lz(2)  *  a 

(2)  *  sin ( theta ( 2 ) )  -  m(2)  *  lz(2)  *  lx(2)  *  sin (theta ( 2 ) )  -  m 

(2)  *  lz(2)  *  ly(2)  *  cos(theta(2) )  -  m(2)  *  d(2)  *  ly(2)  *  cos 

(theta(2))  +  Ixz2  *  sin(theta(2 ) )  +  Iyz2  *  cos ( theta( 2 ) )  -  m(2) 

*  d(2)  *  lx(2)  *  sin( theta( 2 ) ) )  *  alpha(2)  +  (Iyyl  +  0.2el  *  m 

(2)  *  d(2)  *  lz(2)  +  Ixy2  *  sin(0.2el  *  theta(2))  +  m(2)  *  a(l) 

*  2  +  m(2)  *  lz ( 2 )  “  2  +  Iyy2  /  0.2el  +  0.2el  *  m(l)  *  a(l)  * 

lx ( 1 )  +  Iyy2  *  cos ( 0 . 2el  *  theta(2))  /  0.2el  -  m(2)  *  ly(2)  * 

*  cos ( 0 . 2el  *  theta ( 2 ) )  /  0.2el  +  m(2)  *  a(2)  “  2  *  cos(0.2el 
theta(2))  /  0.2el  +  m(2)  *  a(2)  *  lx(2)  *  cos(0.2el  *  theta(2)) 
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CN  * 


-  m(2)  *  a ( 2 )  *  ly(2)  *  sin(0.2el  *  theta(2))  +  m(2)  *  d(2)  A  2 

+  m(2)  *  lx(2)  *  2  *  cos(0.2el  *  theta(2))  /  0.2el  +  0.2el  *  m 
(2)  *  a( 1 )  *  lx (2)  *  cos (theta ( 2 ) )  -  m(2)  *  lx(2)  *  ly(2)  *  sin 

(0.2el  *  theta(2 ) )  +  m(l)  *  a(l)  A  2  +  m(l)  *  lx(l)  A  2  +  m(l) 

*  lz(l)  A  2  -  0 . 2el  *  m(2)  *  a(l)  *  ly(2)  *  sin(theta(2) )  - 

Ixx2  *  cos(0.2el  *  theta(2))  /  0.2el  +  0.2el  *  m(2)  *  a(l)  *  a 

(2)  *  cos ( theta( 2 ) )  +  Ixx2  /  0.2el  +  m(2)  *  a(2)  *  lx(2)  +  m(2) 

*  a(2)  *  2  /  0 . 2el  +  m(2)  *  lx(2)  A  2  /  0.2el  +  m(2)  *  ly(2)  * 

2  /  0.2el)  *  alpha(l)  -  omega(2)  *  2  *  Iyz2  *  sin( theta ( 2 ) )  - 

omega(l)  *  omega(2)  *  m(2)  *  lx (2)  *  2  *  sin(0.2el  *  theta(2)) 

+  omega(2)  *  2  *  Ixz2  *  cos ( theta ( 2 ) )  +  omega(l)  *  omega(2)  *  m 
(2)  *  ly(2)  A  2  *  sin(0.2el  *  theta(2))  +  0.2el  *  omega(l)  * 

omega(2)  *  Ixy2  *  cos(0.2el  *  theta(2))  -  0.2el  *  omega(l)  * 

omega(2)  *  m(2)  *  lx(2)  *  ly(2)  *  cos(0.2el  *  theta(2))  +  omega 
(2)  A  2  *  m(2)  *  d ( 2 )  *  ly(2)  *  sin ( theta ( 2 ) )  -  omega(l)  * 
omega(2)  *  Iyy2  *  sin(0.2el  *  theta(2))  +  omega(l)  *  omega(2)  * 

Ixx2  *  sin(0.2el  *  theta(2))  +  omega(2)  A  2  *  m(2)  *  lz(2)  *  ly 

(2)  *  sin( theta ( 2 ) )  -  omega(2)  A  2  *  m(2)  *  d(2)  *  lx (2)  *  cos 

(theta(2))  -  omega(l)  *  omega(2)  *  m(2)  *  a(2)  A  2  *  sin(0.2el 

*  theta(2))  -  0.2el  *  omega(l)  *  omega(2)  *  m(2)  *  a(l)  *  lx (2) 

*  sin(theta( 2 ) )  -  omega(2)  A  2  *  m(2)  *  lz(2)  *  lx(2)  *  cos 
(theta(2))  -  0.2el  *  omega(l)  *  omega(2)  *  m(2)  *  a(2)  *  ly(2) 

*  cos(0.2el  *  theta(2))  -  omega(2)  A  2  *  m(2)  *  d(2)  *  a(2)  * 
cos ( theta( 2 ) )  -  0.2el  *  omega(l)  *  omega(2)  *  m(2)  *  a(2)  *  lx 
(2)  *  sin(0.2el  *  theta(2))  -  0.2el  *  omega(l)  *  omega(2)  *  m 
(2)  *  a( 1 )  *  a ( 2 )  *  sin ( theta( 2 ) )  -  omega(2)  A  2  *  m(2)  *  lz(2) 

*  a(2)  *  cos ( theta ( 2 ) )  -  0.2el  *  omega(l)  *  omega(2)  *  m(2)  *  a 

(1)  *  ly(2)  *  cos(theta(2) ) ; 

>  T  :=  T  ' 

2  2’ 

T2  :=  (  -m2  fy2  sm(e,j  +  m2  a2  cos  (9^  +  rn^lx,  cos(0, ) )  gc+  (£z2+  2  m2  <7,  A;  +  m2  < 
4-  A;  +  m,  /ic)  a,  +  ( -m,  d2  <z,  sin ( 0,  )  -  ///.,  t2  a^  sin(0,)  -  w,  l~2  A,  sin(0.,) 

-  ///,  b2  cos(  0, )  —  m2  d2  A',  cos(  0, )  +  2xz2 sin ( 0, )  +  Jyz2cos(  0, ) 

2  1  2  1  2 

—  rru,  d2  A,  sin{  0, ) )  otj  +  a>j  a2  h\  cos(  2  0-,)  — -  coj  Ax?  sin  ( 2  0^)  +  —  co~  m, 

2  2  *» 

Zi;sin(2  9,)  +  co“w2^Zi;sin(2e.)  +  co‘ ///,  srnfe.)  +  co‘  ax  Ix\  $in(0,)  + 

CD*  m.  A,  ly2  cos(  2  0, )  +  cd‘  w,  ax  Iy2  cos(  0, )  -  co“  Ax?cos(  2  0,)  +  -^ 

to;  Ar2sin(2  0,)  -  —  to j  w,  Aj  sin (2  0,)  +  coj  m2  a;  sin(2  0,) 

>  CodeGemration\ ' Marla ff ](ZA; 

cg3  =  (-m(2)  *  ly(2)  *  sin( theta( 2 ) )  +  m(2)  *  a(2)  *  cos (theta 

(2) )  +  m(2)  *  lx(2)  *  cos ( theta( 2 ) ) )  *  gc  +  (Izz2  +  0.2el  *  m 
(2)  *  a ( 2 )  *  lx ( 2 )  +  m( 2 )  *  a(2)  A  2  +  m(2)  *  lx(2)  A  2  +  m(2) 

*  ly ( 2 )  A  2)  *  alpha ( 2 )  +  (-m(2)  *  d(2)  *  a(2)  *  sin( theta( 2 ) ) 

-  m(2)  *  lz ( 2 )  *  a ( 2 )  *  sin(theta(2 ) )  -  m(2)  *  lz(2)  *  lx(2)  * 
sin( theta( 2 ) )  -  m(2)  *  lz(2)  *  ly(2)  *  cos ( theta( 2 ) )  -  m(2)  *  d 
(2)  *  ly(2)  *  cos(theta(2) )  +  Ixz2  *  sin ( theta ( 2 ) )  +  Iyz2  *  cos 
( theta ( 2 ) )  -  m(2)  *  d(2)  *  lx(2)  *  sin( theta(2 ) ) )  *  alpha(l)  + 
omega(l)  A  2  *  m(2)  *  a(2)  *  ly(2)  *  cos(0.2el  *  theta(2))  - 
omega(l)  A  2  *  Ixx2  *  sin(0.2el  *  theta(2))  /  0.2el  +  omega(l) 

A  2  *  m(2)  *  lx (2)  A  2  *  sin(0.2el  *  theta(2))  /  0.2el  +  omega 
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(1)  *  2  *  m(2)  *  a ( 2 )  *  lx(2)  *  sin(0.2el  *  theta(2))  +  omega 

(1)  A  2  *  m(2)  *  a(l)  *  a( 2)  *  sin ( theta ( 2 ) )  +  omega(l)  *  2  *  m 

(2)  *  a(l)  *  lx(2)  *  sin(theta(2) )  +  omega(l)  A  2  *  m(2)  *  lx 
(2)  *  ly(2)  *  cos(0.2el  *  theta(2))  +  omega(l)  A  2  *  m(2)  *  a 

(1)  *  ly(2)  *  cos(theta(2) )  -  omega(l)  A  2  *  Ixy2  *  cos(0.2el  * 
theta(2))  +  omega(l)  A  2  *  Iyy2  *  sin(0.2el  *  theta(2))  /  0.2el 

-  omega ( 1 )  A  2  *  m(2)  *  ly(2)  A  2  *  sin(0.2el  *  theta(2))  / 
0.2el  +  omega(l)  A  2  *  m(2)  *  a(2)  A  2  *  s±n(0.2el  *  theta(2)) 

J  0 . 2el ; 

> 

_> 

_> 

>  CodeGenerarion\ 'Mailab' ](  x); 

cg4  =  Iyyl  +  (2  *  m(2)  *  d(2)  *  lz(2))  +  Ixy2  *  sin((2  *  theta 

(2) ))  +  ( m( 2 )  *  a( 1 )  A  2)  +  (m(2)  *  lz(2)  A  2)  +  Iyy2  /  0.2el  + 
(2  *  m(l)  *  a( 1 )  *  lx ( 1 ) )  +  Iyy2  *  cos((2  *  theta(2)))  /  0.2el 

-  m( 2 )  *  ly ( 2 )  A  2  *  cos ( ( 2  *  theta(2)))  /  0.2el  +  m(2)  *  (a(2) 
A  2)  *  cos ( ( 2  *  theta ( 2 ) ) )  /  0.2el  +  m(2)  *  a(2)  *  lx(2)  *  cos( 
(2  *  theta ( 2 ) ) )  -  m(2)  *  a(2)  *  ly(2)  *  sin((2  *  theta(2)))  + 

( m( 2 )  *  d( 2 )  A  2)  +  m( 2 )  *  (lx(2)  A  2)  *  cos((2  *  theta(2)))  / 

0 . 2el  +  0 . 2el  *  m(2)  *  a(l)  *  lx(2)  *  cos ( theta( 2 ) )  -  m(2)  *  lx 
(2)  *  ly ( 2 )  *  sin( ( 2  *  theta(2)))  +  (m(l)  *  a(l)  A  2)  +  (m(l)  * 
lx(l)  A  2)  +  (m(l)  *  lz(l)  A  2)  -  0.2el  *  m(2)  *  a(l)  *  ly(2)  * 
sin(theta(2 ) )  -  Ixx2  *  cos ( ( 2  *  theta(2)))  /  0.2el  +  0.2el  *  m 
(2)  *  a(l)  *  a ( 2 )  *  cos ( theta( 2 ) )  +  Ixx2  /  0.2el  +  (m(2)  *  a(2) 

*  lx ( 2 ) )  +  ( m( 2 )  *  a( 2 )  A  2)  /  0.2el  +  (m(2)  *  lx(2)  A  2)  / 

_0 . 2el  +  m( 2 )  *  ly(2)  '  2  /  0.2el; 

>  CodeGeneration\ 'Matlaff  ](  A/x 

cg5  =  -m(2)  *  d(2)  *  a(2)  *  sin( theta( 2 ) )  -  m(2)  *  lz(2)  *  a(2) 

*  sin ( theta ( 2 ) )  -  m(2)  *  lz(2)  *  lx(2)  *  sin(theta( 2 ) )  -  m(2)  * 
lz(2)  *  ly(2)  *  cos ( theta ( 2 ) )  -  m(2)  *  d(2)  *  ly(2)  *  cos (theta 
(2))  +  Ixz2  *  sin( theta( 2 ) )  +  Iyz2  *  cos ( theta( 2 ) )  -  m(2)  *  d 

_(  2  )  *  lx  ( 2  )  *  sin  ( theta  (  2  )  )  ; 

> 

= 

>  CodeGenerarion\  M,  J; 

cg6  =  -m(2)  *  d(2)  *  a(2)  *  sin ( theta( 2 ) )  -  m(2)  *  lz(2)  *  a(2) 

*  sin ( theta ( 2 ) )  -  m(2)  *  lz(2)  *  lx(2)  *  sin(theta( 2 ) )  -  m(2)  * 
lz(2)  *  ly(2)  *  cos ( theta( 2 ) )  -  m(2)  *  d(2)  *  ly(2)  *  cos (theta 
(2))  +  Ixz2  *  sin(theta(2) )  +  Iyz2  *  cos(theta(2) )  -  m(2)  *  d 

_(  2  )  *  lx(  2  )  *  sin  ( theta  (  2  )  )  ; 

>  CodeGenerarion  \  'Mailad  )  ( J/,  ,  J ; 

cg7  =  Izz2  +  2  *  m( 2 )  *  a(2)  *  lx(2)  +  m(2)  *  a(2)  A  2  +  m(2)  * 
_lx  ( 2 )  A  2  +  m(2)  *  ly  ( 2 )  A  2; 

_> 

>  CodeGeneration\ 'Matlad ](£•); 

_cg8  =  0; 

>  CodeGenerarion\  'Matlaff  ](<?,); 

cg9  =  m(2)  *  (a(2)  *  cos ( theta( 2 ) )  +  lx(2)  *  cos ( theta( 2 ) )  -  ly 
_( 2)  *  sin(theta(2) ) )  *  gc; 

_> 

>  CodeGeneranon  [  'Matlad  ]  ( J ; 

cglO  =  ( -0 . 2el  *  m(2)  *  a(l)  *  lx(2)  *  sin ( theta( 2 ) )  +  0.2el  * 
Ixy2  *  cos(0.2el  *  theta(2))  -  Iyy2  *  sin(0.2el  *  theta(2))  - 
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0.2el  *  m(2)  *  a(l)  *  ly(2)  *  cos ( theta( 2 ) )  -  0.2el  *  m(2)  *  lx 
(2)  *  ly(2)  *  cos(0.2el  *  theta(2))  +  Ixx2  *  sin(0.2el  *  theta 
(2))  -  0 . 2el  *  m( 2 )  *  a(l)  *  a(2)  *  sin( theta(2 ) )  -  m(2)  *  a(2) 

*  2  *  sin( 0 . 2el  *  theta(2))  -  m(2)  *  lx(2)  '  2  *  sin(0.2el  * 
theta ( 2 ) )  +  m(2)  *  ly(2)  A  2  *  sin(0.2el  *  theta(2))  -  0.2el  * 
m( 2 )  *  a( 2 )  *  lx(2)  *  sin(0.2el  *  theta(2))  -  0.2el  *  m(2)  *  a 
(2)  *  ly(2)  *  cos(0.2el  *  theta(2)))  *  omega(l)  *  omega(2)  +  (m 
(2)  *  lz(2)  *  ly(2)  *  sin( theta ( 2 ) )  +m(2)  *  d(2)  *  ly(2)  *  sin 
( theta ( 2 ) )  -  m(2)  *  lz(2)  *  lx(2)  *  cos ( theta( 2 ) )  -  m(2)  *  lz 
(2)  *  a ( 2 )  *  cos ( theta ( 2 ) )  -  m(2)  *  d(2)  *  lx(2)  *  cos(theta(2) 
)  +  Ixz2  *  cos(theta(2) )  -  Iyz2  *  sin( theta( 2 ) )  -  m(2)  *  d(2)  * 

_a(2)  *  cos  ( theta (  2  )  )  )  *  omega(2)  A  2; 

>  CodeGenerarion\ ' Marla ff  ]  ( J ; 

cgll  =  (m(2)  *  a(l)  *  lx(2)  *  sin( theta( 2 ) )  -  Ixy2  *  cos(0.2el 

*  theta ( 2 ) )  +  Iyy2  *  sin(0.2el  *  theta<2))  /  0.2el  +  m(2)  *  a 

(1)  *  ly ( 2 )  *  cos(theta(2) )  +  m(2)  *  lx(2)  *  ly(2)  *  cos(0.2el 

*  theta ( 2 ) )  -  Ixx2  *  sin(0.2el  *  theta(2))  /  0.2el  +  m(2)  *  a 

(1)  *  a ( 2 )  *  s in ( theta ( 2 ) )  +  m(2)  *  a(2)  A  2  *  sin(0.2el  * 

theta ( 2 ) )  /  0.2el  +  m(2)  *  lx(2)  *  2  *  sin(0.2el  *  theta(2))  / 
0.2el  -  m(2)  *  ly(2)  A  2  *  sin(0.2el  *  theta(2))  /  0.2el  +  m(2) 

*  a( 2 )  *  lx( 2 )  *  sin( 0 . 2el  *  theta(2))  +  m(2)  *  a(2)  *  ly(2)  * 
cos ( 0 . 2el  *  theta ( 2 ) ) )  *  omega(l)  A  2  +  (m(2)  *  lz(2)  *  ly(2)  * 
sin (theta (2 ) )  /  0.2el  +m(2)  *  d(2)  *  ly(2)  *  sin( theta ( 2 ) )  / 

0 . 2el  -  m( 2 )  *  lz(2)  *  lx(2)  *  cos ( theta( 2 ) )  /  0.2el  -  m(2)  * 

lz ( 2 )  *  a( 2 )  *  cos (theta (2 ) )  /  0.2el  -  m(2)  *  d(2)  *  lx(2)  * 

cos ( theta( 2 ) )  /  0.2el  +  Ixz2  *  cos ( theta( 2 ) )  /  0.2el  -  Iyz2  * 

sin (theta (2 ) )  /  0.2el  -  m(2)  *  d(2)  *  a(2)  *  cos ( theta ( 2 ) )  / 

0.2el)  *  omega(l)  *  omega(2)  +  (-m(2)  *  lz(2)  *  ly(2)  *  sin 
( theta ( 2 ) )  /  0.2el  -  m(2)  *  d(2)  *  ly(2)  *  sin(theta(2 ) )  / 

0.2el  +  m( 2 )  *  lz(2)  *  lx(2)  *  cos ( theta( 2 ) )  /  0.2el  +  m(2)  * 

lz ( 2 )  *  a( 2 )  *  cos ( theta ( 2 ) )  /  0.2el  +  m(2)  *  d(2)  *  lx(2)  * 

cos ( theta( 2 ) )  /  0.2el  -  Ixz2  *  cos ( theta( 2 ) )  /  0.2el  +  Iyz2  * 

sin(theta(2) )  /  0.2el  +m(2)  *  d(2)  *  a(2)  *  cos ( theta ( 2 ) )  / 

_0.2el)  *  omega(2)  *  omega(l); 

> 
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